Posture estimation method, posture estimation device, and vehicle

ABSTRACT

A posture estimation method includes calculating a posture change amount of an object based on an output of an angular velocity sensor, predicting posture information of the object by using the posture change amount, adjusting error information in a manner of determining whether or not the output of the angular velocity sensor is within an effective range and, when it is determined that the output of the angular velocity sensor is not within the effective range, increasing a posture error component in error information and reducing a correlation component between the posture error component and an error component other than the posture error component in the error information, and correcting the predicted posture information of the object based on the error information.

The present application is based on, and claims priority from JPApplication Serial Number 2018-143691, filed Jul. 31, 2018, thedisclosure of which is hereby incorporated by reference herein in itsentirety.

BACKGROUND 1. Technical Field

The present disclosure relates to a posture estimation method, a postureestimation device, and a vehicle.

2. Related Art

A device or a system of mounting an inertial measurement unit (IMU) onan object and calculating the position or posture of the object with anoutput signal of the inertial measurement unit (IMU) is known. A biaserror is provided in the output signal of the inertial measurement unit(IMU), and an error also occurs in posture calculation. Thus, atechnique of correcting such an error with a Kalman filter andestimating the accurate posture of the object is proposed. For example,JP-A-2015-179002 discloses a posture estimation method of calculating aposture change amount of an object by using an output of an angularvelocity sensor and estimating the posture of the object by using theposture change amount.

JP-A-2015-179002 is an example of the related art.

An effective measurement range is defined in an angular velocity sensor.When an angular velocity which is out of the range is temporarily input,the output of the angular velocity sensor may include a not-assumedlarge error. Regarding this, in the posture estimation method disclosedin JP-A-2015-179002, there is a concern of not assuming a case where theoutput of the angular velocity sensor is even temporarily out of theeffective range and decreasing accuracy in estimating the posture of anobject.

SUMMARY

An aspect of a posture estimation method according to the presentdisclosure includes calculating a posture change amount of an objectbased on an output of an angular velocity sensor, predicting postureinformation of the object by using the posture change amount, adjustingerror information in a manner of determining whether or not the outputof the angular velocity sensor is within an effective range, and when itis determined that the output of the angular velocity sensor is notwithin the effective range, increasing a posture error component inerror information and reducing a correlation component between theposture error component and an error component other than the postureerror component in the error information, and correcting the predictedposture information of the object based on the error information.

In the aspect of the posture estimation method, the adjusting of theerror information may include determining whether or not the currenttime is in a first period after it is determined that the output of theangular velocity sensor is not within the effective range, increasingthe posture error component in the first period, and reducing thecorrelation component between the posture error component and the errorcomponent other than the posture error component, in the first period.

In the aspect of the posture estimation method, the error componentother than the posture error component may include a bias errorcomponent of an angular velocity.

In the aspect of the posture estimation method, the correlationcomponent between the posture error component and the error componentother than the posture error component may be zero.

The aspect of the posture estimation method may further includecalculating a velocity change amount of the object based on an output ofan acceleration sensor and the output of the angular velocity sensor. Inthe adjusting of the error information, whether or not the output of theacceleration sensor is within an effective range may be determined, and,when it is determined that the output of the angular velocity sensor orthe output of the acceleration sensor is not within the correspondingeffective range, a motion velocity error component in the errorinformation may be increased, and a correlation component between themotion velocity error component and an error component other than themotion velocity error component in the error information may be reduced.In the predicting of the posture information, velocity information ofthe object may be predicted by using the velocity change amount.

In the aspect of the posture estimation method, the adjusting of theerror information may include determining whether or not the currenttime is in a second period after it is determined that the output of theacceleration sensor is not within the effective range, increasing themotion velocity error component in the second period, and reducing thecorrelation component between the motion velocity error component andthe error component other than the motion velocity error component, inthe second period.

In the aspect of the posture estimation method, the error componentother than the motion velocity error component may include a bias errorcomponent of an acceleration.

In the aspect of the posture estimation method, the correlationcomponent between the motion velocity error component and the errorcomponent other than the motion velocity error component may be zero.

An aspect of a posture estimation device according to the presentdisclosure includes a posture-change-amount calculation unit thatcalculates a posture change amount of an object based on an output of anangular velocity sensor, a posture information prediction unit thatpredicts posture information of the object by using the posture changeamount, an error information adjustment unit that determines whether ornot the output of the angular velocity sensor is within an effectiverange, and when it is determined that the output of the angular velocitysensor is not within the effective range, increases a posture errorcomponent in error information, and reduces a correlation componentbetween the posture error component and an error component other thanthe posture error component in the error information, and a postureinformation correction unit that corrects the predicted postureinformation of the object based on the error information.

An aspect of a vehicle according to the present disclosure includes theaspect of the posture estimation device and a control device thatcontrols a posture of the vehicle based on posture information of thevehicle, which is estimated by the posture estimation device.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is a flowchart illustrating an example of procedures of a postureestimation method.

FIG. 2 is a flowchart illustrating an example of procedures of a processS3 in FIG. 1.

FIG. 3 is a flowchart illustrating an example of procedures of a processS5 in FIG. 1.

FIG. 4 is a flowchart illustrating an example of procedures of a processS6 in FIG. 1.

FIG. 5 is a flowchart illustrating an example of procedures of a processS7 in FIG. 1.

FIG. 6 is a diagram illustrating an example of a configuration of aposture estimation device according to an embodiment.

FIG. 7 is a diagram illustrating a sensor coordinate system and a localcoordinate system.

FIG. 8 is a diagram illustrating an example of a configuration of aprocessing unit in the embodiment.

FIG. 9 is a flowchart illustrating another example of the procedures ofthe process S5 in FIG. 1.

FIG. 10 is a block diagram illustrating an example of a configuration ofan electronic device in the embodiment.

FIG. 11 is a plan view illustrating a wrist watch-type activity meter asa portable type electronic device.

FIG. 12 is a block diagram illustrating an example of a configuration ofthe wrist watch-type activity meter as the portable type electronicdevice.

FIG. 13 illustrates an example of a vehicle in the embodiment.

FIG. 14 is a block diagram illustrating an example of a configuration ofthe vehicle.

FIG. 15 illustrates another example of the vehicle in the embodiment.

FIG. 16 is a block diagram illustrating an example of a configuration ofthe vehicle.

DESCRIPTION OF EXEMPLARY EMBODIMENTS

Hereinafter, a preferred embodiment according to the present disclosurewill be described in detail with reference to the drawings. Embodimentsdescribed below do not unduly limit the contents of the presentdisclosure described in the appended claims. All components describedbelow are not essential components in the present disclosure.

1. Posture Estimation Method 1-1. Posture Estimation Theory 1-1-1. IMUOutput Model

The output of the inertial measurement unit (IMU) includes angularvelocity data d_(ω, k) as an output of a three-axis angular velocitysensor and acceleration data d_(α, k) as an output of a three-axisacceleration sensor at each sampling time point (t_(k)). Here, as shownin Expression (1), the angular velocity data d_(ω, k) is represented bythe sum of an average value of an angular velocity vector ω in asampling interval (Δt=t_(k)−t_(k−1)) and the residual bias b_(ω).

$\begin{matrix}{{d_{\omega,k} = {\begin{bmatrix}d_{{\omega \; x},k} \\d_{{\omega \; y},k} \\d_{{\omega \; z},k}\end{bmatrix} = {{\overset{\_}{\omega}}_{k} + b_{\omega}}}},{{\overset{\_}{\omega}}_{k} = {\frac{1}{\Delta \; t}{\int_{t_{k - 1}}^{t_{k}}{\omega \; {dt}}}}}} & (1)\end{matrix}$

Similarly, as shown in Expression (2), the acceleration data d_(α, k) isalso represented by the sum of an average value of an accelerationvector α and the residual bias b_(α).

$\begin{matrix}{{d_{\alpha,k} = {\begin{bmatrix}d_{{\alpha \; x},k} \\d_{{\alpha \; y},k} \\d_{{\alpha \; z},k}\end{bmatrix} = {{\overset{\_}{\alpha}}_{k} + b_{\alpha}}}},{{\overset{\_}{\alpha}}_{k} = {\frac{1}{\Delta \; t}{\int_{t_{k - 1}}^{t_{k}}{\alpha \; {dt}}}}}} & (2)\end{matrix}$

1-1-2. Calculation of Three-Dimensional Posture by Angular VelocityIntegration

When a three-dimensional posture is represented by quaternions, arelation between the posture quaternion q and the angular velocityvector ω [rad/s] is represented by a differential equation in Expression(3).

$\begin{matrix}{{\frac{d}{dt}q} = {\frac{1}{2}{q \otimes \omega}}} & (3)\end{matrix}$

Here, a symbol obtained by superimposing ◯ and × on each other indicatesquaternion multiplication. For example, elements of quaternionmultiplication of q and p are calculated as shown in Expression (4).

$\begin{matrix}{{q \otimes p} = {{\begin{bmatrix}{+ q_{0}} & {- q_{1}} & {- q_{2}} & {- q_{3}} \\{+ q_{1}} & {+ q_{0}} & {- q_{3}} & {+ q_{2}} \\{+ q_{2}} & {+ q_{3}} & {+ q_{0}} & {- q_{1}} \\{+ q_{3}} & {- q_{2}} & {+ q_{1}} & {+ q_{0}}\end{bmatrix}\begin{bmatrix}p_{0} \\p_{1} \\p_{2} \\p_{3}\end{bmatrix}} = {\quad\begin{bmatrix}{{q_{0}p_{0}} - {q_{1}p_{1}} - {q_{2}p_{2}} - {q_{3}p_{3}}} \\{{q_{1}p_{0}} + {q_{0}p_{1}} - {q_{3}p_{2}} + {q_{2}p_{3}}} \\{{q_{2}p_{0}} + {q_{3}p_{1}} + {q_{0}p_{2}} - {q_{1}p_{3}}} \\{{q_{3}p_{0}} - {q_{2}p_{1}} + {q_{1}p_{2}} + {q_{0}p_{3}}}\end{bmatrix}}}} & (4)\end{matrix}$

As shown in Expression (5), the angular velocity vector ω is consideredas being equivalent to a quaternion in which the real (scalar) componentis zero, and the imaginary (vector) component coincides with thecomponent of w.

$\begin{matrix}{\omega = {{\begin{bmatrix}\omega_{x} \\\omega_{y} \\\omega_{z}\end{bmatrix} \equiv \begin{bmatrix}0 \\\omega_{x} \\\omega_{y} \\\omega_{z}\end{bmatrix}} = \begin{bmatrix}0 \\\omega\end{bmatrix}}} & (5)\end{matrix}$

If the differential equation (3) is solved, it is possible to calculatethe three-dimensional posture. However, unfortunately, the generalsolution thereof has not been found. Further, the value of the angularvelocity vector ω is also obtained in only a form of a discrete averagevalue. Thus, it is necessary that approximation calculation is performedwith Expression (6) for each short (sampling) time.

$\begin{matrix}{q_{k} \approx {{q_{k - 1} \otimes \Delta}\; q_{k}\left\{ \begin{matrix}{{\Delta \; q_{k}} = {{1 + {\frac{{\overset{\_}{\omega}}_{k}}{2}\Delta \; t} + {\left( {\frac{{\overset{\_}{\omega}}_{k - 1} \times {\overset{\_}{\omega}}_{k}}{24} - \frac{{{\overset{\_}{\omega}}_{k}}^{2}}{8}} \right)\Delta \; t^{2}} - {\frac{{{\overset{\_}{\omega}}_{k}}^{2}{\overset{\_}{\omega}}_{k}}{48}\Delta \; t^{3}}} =}} \\\begin{bmatrix}{1 - {\frac{1}{8}{{{\overset{\_}{\omega}}_{k}\Delta \; t}}^{2}}} \\\begin{matrix}{{\frac{1}{2}{\overset{\_}{\omega}}_{k}\Delta \; t} + {\frac{1}{24}\left( {{\overset{\_}{\omega}}_{k - 1}\Delta \; t \times {\overset{\_}{\omega}}_{k}\Delta \; t} \right)} -} \\{\frac{1}{48}{{{\overset{\_}{\omega}}_{k}\Delta \; t}}^{2}{\overset{\_}{\omega}}_{k}\Delta \; t}\end{matrix}\end{bmatrix}\end{matrix} \right.}} & (6)\end{matrix}$

Expression (6) is an expression calculated based on Taylor expansion ateach of t=t_(k−1) to the third-order term of Δt, considering the posturequaternion q and an integration relation of the angular velocity vectorω for each axis. The term including ω_(k−1) in the expressioncorresponds to the Corning correction term. The symbol × indicates across product (vector product) of a three-dimensional vector. Forexample, the elements of v×w are calculated as in Expression (7).

$\begin{matrix}{{v \times w} = {{\begin{bmatrix}0 & {- v_{z}} & {+ v_{y}} \\{+ v_{2}} & 0 & {- v_{x}} \\{- v_{y}} & {+ v_{z}} & 0\end{bmatrix}\begin{bmatrix}w_{x} \\w_{y} \\w_{z}\end{bmatrix}} = \begin{bmatrix}{{v_{y}w_{z}} - {v_{z}w_{y}}} \\{{v_{z}w_{x}} - {v_{x}w_{z}}} \\{{v_{x}w_{y}} - {v_{y}w_{x}}}\end{bmatrix}}} & (7)\end{matrix}$

1-1-3. Tilt Error Observation by Gravitational Acceleration

The acceleration sensor detects an acceleration generated by themovement thereof. However, on the earth, the acceleration is normallydetected in a state of adding a gravitational acceleration of about 1[G] (=9.80665 [m/s²]). The gravitational acceleration is normally avector in a vertical direction. Thus, it is possible to know an error ofa tilt (roll and pitch) component of the posture by comparison to theoutput of the three-axis acceleration sensor. Therefore, firstly, it isnecessary that an acceleration vector α in the sensor coordinate system(xyz coordinate system), which is observed by the three-axisacceleration sensor is transformed to an acceleration vector α′ in acoordinate system (XYZ coordinate system) of a local space obtained byhorizontal orthogonal axes and a vertical axis. The coordinate(rotation) transformation can be calculated with the posture quaternionq and conjugate quaternion q*, as shown in Expression (8).

α′=q⊗α⊗q*  (8)

Expression (8) can be expressed with a three-dimensional coordinatetransformation matrix C, as in Expression (9).

$\begin{matrix}{\alpha^{\prime} = {C\; \alpha \left\{ {C = \begin{bmatrix}{q_{0}^{2} + q_{1}^{2} - q_{2}^{2} - q_{3}^{2}} & {2\left( {{q_{1}q_{2}} - {q_{0}q_{3}}} \right)} & {2\left( {{q_{1}q_{3}} + {q_{0}q_{2}}} \right)} \\{2\left( {{q_{1}q_{2}} + {q_{0}q_{3}}} \right)} & {q_{0}^{2} - q_{1}^{2} + q_{2}^{2} - q_{3}^{2}} & {2\left( {{q_{2}q_{3}} - {q_{0}q_{1}}} \right)} \\{2\left( {{q_{1}q_{3}} - {q_{0}q_{2}}} \right)} & {2\left( {{q_{2}q_{3}} + {q_{0}q_{1}}} \right)} & {q_{0}^{2} - q_{1}^{2} - q_{2}^{2} + q_{3}^{2}}\end{bmatrix}} \right.}} & (9)\end{matrix}$

A tilt error is obtained by comparing the acceleration vector α′ to thegravitational acceleration vector g in the local-space coordinate system(XYZ coordinate system). The gravitational acceleration vector g isrepresented by Expression (10). In Expression (10), Δg indicates agravitational-acceleration correction value indicating a difference [G]from the standard value of the gravitational acceleration vector g.

$\begin{matrix}{g = \begin{bmatrix}0 \\0 \\{- \left( {1 + {\Delta \; g}} \right)}\end{bmatrix}} & (10)\end{matrix}$

1-1-4. Observation of Zero Motion Velocity

In particular, the motion velocity of the IMU is considered to besubstantially equal to zero in a long term, in user interfaceapplications. A relation between the motion velocity vector v in thelocal-space coordinate system, and the acceleration vector α and theangular velocity vector ω in the sensor coordinate system is expressedwith the coordinate transformation matrix C by a differential equationin Expression (11).

$\begin{matrix}{{{\frac{d}{dt}v} = {{C\; \alpha} - g}}{{\frac{d}{dt}C} = {C\begin{bmatrix}0 & {- \omega_{z}} & {+ \omega_{y}} \\{+ \omega_{z}} & 0 & {- \omega_{x}} \\{- \omega_{y}} & {+ \omega_{x}} & 0\end{bmatrix}}}} & (11)\end{matrix}$

Here, the values of the acceleration vector α and the angular velocityvector ω are obtained only in a form of a discrete average value. Thus,the motion velocity vector is calculated by performing approximationcalculation with Expression (12) for each short (sampling) time.

$\begin{matrix}{v_{k} \approx {v_{k - 1} + {\left( {{C_{k}\lambda_{k}} - g} \right)\Delta \; t\left\{ \begin{matrix}{\lambda_{k} = {{\overset{\_}{\alpha}}_{k} + {\left( {\frac{{\overset{\_}{\omega}}_{k} \times {\overset{\_}{\alpha}}_{k}}{2} + \frac{{{\overset{\_}{\omega}}_{k - 1} \times {\overset{\_}{\alpha}}_{k}} - {{\overset{\_}{\omega}}_{k} \times {\overset{\_}{\alpha}}_{k - 1}}}{12}} \right)\Delta \; t} + {\frac{{\left( {{\overset{\_}{\omega}}_{k} \cdot {\overset{\_}{\alpha}}_{k}} \right){\overset{\_}{\omega}}_{k}} - {{{\overset{\_}{\omega}}_{k}}^{2}{\overset{\_}{\alpha}}_{k}}}{6}\Delta \; t^{2}} +}} \\{{{\frac{{\left( {{{\overset{\_}{\omega}}_{k - 1} \cdot {\overset{\_}{\alpha}}_{k}} - {{\overset{\_}{\omega}}_{k} \cdot {\overset{\_}{\alpha}}_{k - 1}}} \right){\overset{\_}{\omega}}_{k}} - {\left( {{\overset{\_}{\omega}}_{k - 1} \cdot {\overset{\_}{\omega}}_{k}} \right){\overset{\_}{\alpha}}_{k}} + {{{\overset{\_}{\omega}}_{k}}^{2}{\overset{\_}{\alpha}}_{k - 1}}}{24}\Delta \; t^{2}} - {\frac{{{\overset{\_}{\omega}}_{k}}^{2}}{24}{\overset{\_}{\omega}}_{k} \times {\overset{\_}{\alpha}}_{k}\Delta \; t^{3}}} =} \\{{\overset{\_}{\alpha}}_{k} + {\frac{1}{2}\left( {{\overset{\_}{\omega}}_{k}\Delta \; t \times {\overset{\_}{\alpha}}_{k}} \right)} + {\frac{1}{12}\left( {{\overset{\_}{\omega}}_{k - 1}\Delta \; t \times {\overset{\_}{\alpha}}_{k}} \right)} - {\frac{1}{12}\left( {{\overset{\_}{\omega}}_{k}\Delta \; t \times {\overset{\_}{\alpha}}_{k - 1}} \right)} + {\frac{1}{6}\left( {{\overset{\_}{\omega}}_{k}\Delta \; t \times \left( {{\overset{\_}{\omega}}_{k}\Delta \; t \times {\overset{\_}{\alpha}}_{k}} \right)} \right)} +} \\{{\frac{1}{24}\left( {{\overset{\_}{\omega}}_{k - 1}\Delta \; t \times \left( {{\overset{\_}{\omega}}_{k}\Delta \; t \times {\overset{\_}{\alpha}}_{k}} \right)} \right)} - {\frac{1}{24}\left( {{\overset{\_}{\omega}}_{k}\Delta \; t \times \left( {{\overset{\_}{\omega}}_{k}\Delta \; t \times {\overset{\_}{\alpha}}_{k - 1}} \right)} \right)} - {\frac{1}{24}{{{\overset{\_}{\omega}}_{k}\Delta \; t}}^{2}\left( {{\overset{\_}{\omega}}_{k}\Delta \; t \times {\overset{\_}{\alpha}}_{k}} \right)}}\end{matrix} \right.}}} & (12)\end{matrix}$

Expression (12) is an expression calculated based on Taylor expansion ateach of t=t_(k−1) to the third-order term of Δt, considering the motionvelocity vector v and integration relations of the acceleration vector αand the angular velocity vector ω for each axis. The third-order term isignored in Expression (12) because of being sufficiently small althoughthe residual error ε_(λ) is provided in the third-order term. The symbol· indicates a dot product (scalar product) of a three-dimensionalvector. For example, v·w is calculated as in Expression (13).

v·w=r _(x) w _(x) +r _(y) w _(y) +v _(z) w _(z)  (13)

1-1-5. Posture Quaternion and Error Thereof

It is considered that the true value ({circumflex over ( )}) of thecalculated posture quaternion q has an error ε_(q), as in Expression(14).

$\begin{matrix}{{q = {{\hat{q} + \epsilon_{q}} = \begin{bmatrix}{{\hat{q}}_{0} + \epsilon_{q\; 0}} \\{{\hat{q}}_{1} + \epsilon_{q\; 1}} \\{{\hat{q}}_{2} + \epsilon_{q\; 2}} \\{{\hat{q}}_{3} + \epsilon_{q\; 3}}\end{bmatrix}}}{\sum\limits_{q}^{2}{= {E\left\lbrack {\epsilon_{q}\epsilon_{q}^{T}} \right\rbrack}}}} & (14)\end{matrix}$

Here, Σ_(q) ² represents an error covariance matrix indicating themagnitude of the error ε_(q). E[⋅] indicates an expected value. T on theright shoulder indicates transposition of the vector-matrix.

The quaternion and the error have four values, but there are just threedegrees of freedom in a three-dimensional posture (rotationaltransformation). The fourth degree of freedom of the posture quaternioncorresponds to enlargement/reduction conversion. However, it isnecessary that the enlargement/reduction ratio is normally fixed to 1 inposture detection processing. In practice, a enlargement/reduction ratiocomponent changes by various calculation errors. Thus, processing ofsuppressing the change of the enlargement/reduction ratio is required.

In a case of the posture quaternion q, the square of the absolute valuethereof corresponds to the enlargement/reduction ratio. Thus, the changeis suppressed by normalization processing in which the absolute value isset to 1, as in Expression (15).

$\begin{matrix}{\left. q\leftarrow\frac{q}{q} \right. = {\frac{1}{\sqrt{q_{0}^{2} + q_{1}^{2} + q_{2}^{2} + q_{3}^{2}}}\begin{bmatrix}q_{0} \\q_{1} \\q_{2} \\q_{3}\end{bmatrix}}} & (15)\end{matrix}$

In a case of the posture error ε_(q), it is necessary to hold the rank(order number) of the error covariance matrix Σ_(q) ² to be three. Thus,the rank is limited as in Expression (16), considering a(three-dimensional) error rotation vector ε_(θ) in the local-spacecoordinate system (assuming that a posture error angle is sufficientlysmall).

$\begin{matrix}{{\epsilon_{\theta} = {\begin{bmatrix}\epsilon_{\theta \; x} \\\epsilon_{\theta \; y} \\\epsilon_{\theta \; z}\end{bmatrix} \approx {2D\; \epsilon_{q}}}}\left. \epsilon_{q}\leftarrow{D^{T}D\; \epsilon_{q}} \right.{\sum\limits_{q}^{2}\left. \leftarrow{D^{T}D{\sum\limits_{q}^{2}{D^{T}D}}} \right.}} & (16) \\\left\{ \begin{matrix}{D = \begin{bmatrix}{- q_{1}} & {+ q_{0}} & {- q_{3}} & {+ q_{2}} \\{- q_{2}} & {+ q_{3}} & {+ q_{0}} & {- q_{1}} \\{- q_{3}} & {- q_{2}} & {+ q_{1}} & {+ q_{0}}\end{bmatrix}} \\{{D^{T}D} = \begin{bmatrix}{q_{1}^{2} + q_{2}^{2} + q_{3}^{2}} & {{- q_{0}}q_{1}} & {{- q_{0}}q_{2}} & {{- q_{0}}q_{3}} \\{{- q_{0}}q_{1}} & {q_{0}^{2} + q_{2}^{2} + q_{3}^{2}} & {{- q_{1}}q_{2}} & {{- q_{1}}q_{3}} \\{{- q_{0}}q_{2}} & {{- q_{1}}q_{2}} & {q_{0}^{2} + q_{1}^{2} + q_{3}^{2}} & {{- q_{2}}q_{3}} \\{{- q_{0}}q_{3}} & {{- q_{1}}q_{3}} & {{- q_{2}}q_{3}} & {q_{0}^{2} + q_{1}^{2} + q_{2}^{2}}\end{bmatrix}}\end{matrix} \right. & \;\end{matrix}$

1-1-6. Removal (Ignoring) of Azimuth Error

When an azimuth observation section such as a magnetic sensor is notprovided in posture detection, an azimuthal component of the postureerror just monotonously increases and does not serve for any purpose.Further, the increased error estimate causes a feedback gain tounnecessarily increase, and this causes the azimuth to unexpectedlychange or vary. Thus, an azimuth error component ε_(θz) is removed fromExpression (16).

$\begin{matrix}{\mspace{20mu} {{\epsilon_{\theta}^{\prime} = {\begin{bmatrix}\epsilon_{\theta \; x} \\\epsilon_{\theta \; y}\end{bmatrix} \approx {2D^{\prime}\epsilon_{q}}}}\mspace{20mu} \left. \epsilon_{q}\leftarrow{D^{\prime \; T}D^{\prime}\; \epsilon_{q}} \right.\mspace{20mu} {\sum\limits_{q}^{2}\left. \leftarrow{D^{\prime \; T}D^{\prime}{\sum\limits_{q}^{2}{D^{\prime \; T}D^{\prime}}}} \right.}\left\{ \begin{matrix}{D^{\prime} = \begin{bmatrix}{- q_{1}} & {+ q_{0}} & {- q_{3}} & {+ q_{2}} \\{- q_{2}} & {+ q_{3}} & {+ q_{0}} & {- q_{1}}\end{bmatrix}} \\{{D^{\prime \; T}D^{\prime}} = \begin{bmatrix}{q_{1}^{2} + q_{2}^{2}} & {{{- q_{0}}q_{1}} - {q_{2}q_{3}}} & {{{- q_{1}}q_{3}} - {q_{0}q_{2}}} & 0 \\{{{- q_{0}}q_{1}} - {q_{2}q_{3}}} & {q_{0}^{2} + q_{3}^{2}} & 0 & {{q_{0}q_{2}} - {q_{1}q_{3}}} \\{{q_{1}q_{3}} - {q_{0}q_{2}}} & 0 & {q_{0}^{2} + q_{3}^{2}} & {{{- q_{0}}q_{1}} - {q_{2}q_{3}}} \\0 & {{q_{0}q_{2}} - {q_{1}q_{3}}} & {{{- q_{0}}q_{1}} - {q_{2}q_{3}}} & {q_{1}^{2} + q_{2}^{2}}\end{bmatrix}}\end{matrix} \right.}} & (17)\end{matrix}$

1-1-7. Extended Kalman Filter

An extended Kalman filter that calculates a three-dimensional posturebased on the above model expressions can be designed.

State Vector and Error Covariance Matrix

As in Expression (18), the posture quaternion q, the motion velocityvector v, the residual bias b_(ω) (offset to angular velocity vector ω)of the angular velocity sensor, the residual bias b_(α) (offset toacceleration vector α) of the acceleration sensor, and thegravitational-acceleration correction value Δg, as unknown state valuesto be obtained, constitute a state vector x (14-dimensional vector) ofthe extended Kalman filter. In addition, the error covariance matrixΣ_(x) ² is defined.

$\begin{matrix}{{{x = \begin{bmatrix}q \\v \\b_{\omega} \\b_{\alpha} \\{\Delta \; g}\end{bmatrix}},{\sum\limits_{x}^{2}{= {E\left\lbrack {\epsilon_{x}\epsilon_{x}^{T}} \right\rbrack}}},{\epsilon_{x} = \begin{bmatrix}\epsilon_{q} \\\epsilon_{v} \\\epsilon_{b\; \omega} \\\epsilon_{b\; \alpha} \\\epsilon_{\Delta \; g}\end{bmatrix}}}{x = {\hat{x} + \epsilon_{x}}}} & (18)\end{matrix}$

Process Model

In a process model, the value of the latest state vector is predictedbased on the sampling interval Δt and values of the angular velocitydata d_(ω) and the acceleration data d_(α), as in Expression (19).

$\begin{matrix}{x_{k} = {\begin{bmatrix}q_{k} \\v_{k} \\b_{\omega,k} \\b_{\alpha,k} \\{\Delta \; g_{k}}\end{bmatrix} = {\begin{bmatrix}{{q_{k - 1} \otimes \Delta}\; q_{k}} \\{v_{k - 1} + {\left( {{C_{k}\lambda_{k}} - g_{k - 1}} \right)\Delta \; t}} \\b_{\omega,{k - 1}} \\b_{\alpha,{k - 1}} \\{\Delta \; g_{k - 1}}\end{bmatrix}\left\{ \begin{matrix}{{\Delta \; q_{k}} = {1 + {\frac{{\overset{\_}{\omega}}_{k}}{2}\Delta \; t} + {\left( {\frac{{\overset{\_}{\omega}}_{k - 1} \times {\overset{\_}{\omega}}_{k}}{24} - \frac{{{\overset{\_}{\omega}}_{k}}^{2}}{8}} \right)\Delta \; t^{2}} - {\frac{{{\overset{\_}{\omega}}_{k}}^{2}{\overset{\_}{\omega}}_{k}}{48}\Delta \; t^{3}}}} \\{C_{k + 1} = \begin{bmatrix}{q_{0,k}^{2} + q_{1,k}^{2} - q_{2,k}^{2} - q_{3,k}^{2}} & {2\left( {{q_{1,k}q_{2,k}} - {q_{0,k}q_{3,k}}} \right)} & {2\left( {{q_{1,k}q_{3,k}} + {q_{0,k}q_{2,k}}} \right)} \\{2\left( {{q_{1,k}q_{2,k}} + {q_{0,k}q_{3,k}}} \right)} & {q_{0,k}^{2} - q_{1,k}^{2} + q_{2,k}^{2} - q_{3,k}^{2}} & {2\left( {{q_{2,k}q_{3,k}} - {q_{0,k}q_{1,k}}} \right)} \\{2\left( {{q_{1,k}q_{3,k}} - {q_{0,k}q_{2,k}}} \right)} & {2\left( {{q_{2,k}q_{3,k}} + {q_{0,k}q_{1,k}}} \right)} & {q_{0,k}^{2} - q_{1,k}^{2} - q_{2,k}^{2} + q_{3,k}^{2}}\end{bmatrix}} \\{\lambda_{k} = {{\overset{\_}{\alpha}}_{k} + {\left( {\frac{{\overset{\_}{\omega}}_{k} \times {\overset{\_}{\alpha}}_{k}}{2} + \frac{{{\overset{\_}{\omega}}_{k - 1} \times {\overset{\_}{\alpha}}_{k}} - {{\overset{\_}{\omega}}_{k} \times {\overset{\_}{\alpha}}_{k - 1}}}{12}} \right)\Delta \; t} + {\frac{{\left( {{\overset{\_}{\omega}}_{k} \cdot {\overset{\_}{\alpha}}_{k}} \right){\overset{\_}{\omega}}_{k}} - {{{\overset{\_}{\omega}}_{k}}^{2}{\overset{\_}{\alpha}}_{k}}}{6}\Delta \; t^{2}} +}} \\{{\frac{{\left( {{{\overset{\_}{\omega}}_{k - 1} \cdot {\overset{\_}{\alpha}}_{k}} - {{\overset{\_}{\omega}}_{k} \cdot {\overset{\_}{\alpha}}_{k - 1}}} \right){\overset{\_}{\omega}}_{k}} - {\left( {{\overset{\_}{\omega}}_{k - 1} \cdot {\overset{\_}{\omega}}_{k}} \right){\overset{\_}{\alpha}}_{k}} + {{{\overset{\_}{\omega}}_{k}}^{2}{\overset{\_}{\alpha}}_{k - 1}}}{24}\Delta \; t^{2}} - {\frac{{{\overset{\_}{\omega}}_{k}}^{2}}{24}{\overset{\_}{\omega}}_{k} \times {\overset{\_}{\alpha}}_{k}\Delta \; t^{3}}} \\{g_{k} = \begin{bmatrix}0 & 0 & {- \left( {1 + {\Delta \; g_{k}}} \right)}\end{bmatrix}^{T}} \\{{\overset{\_}{\omega}}_{k} = {d_{\omega,k} - b_{\omega,{k - 1}}}} \\{{\overset{\_}{\alpha}}_{k} = {d_{\alpha,k} - b_{\alpha,{k - 1}}}}\end{matrix} \right.}}} & (19)\end{matrix}$

The covariance matrix of a state error is updated as in Expression (20)by receiving influences of noise components η_(ω) and η_(α) of theangular velocity data d_(ω), and the acceleration data d_(α), andprocess noise ρ_(ω), ρ_(α), and ρ_(g) indicating instability of theresidual bias b_(ω) of the angular velocity sensor, the residual biasb_(α) of the acceleration sensor, and the value (gravitationalacceleration value) of the gravitational acceleration vector g.

$\begin{matrix}{\overset{2}{\sum\limits_{x,k}}{= {{A_{k}{\sum\limits_{x,{k - 1}}^{2}A_{k}^{T}}} + {W_{k}{\sum\limits_{\rho,k}^{2}{W_{k}^{T}\left\{ \begin{matrix}{A_{k} \approx \begin{bmatrix}J_{{q/q},k} & 0_{4 \times 3} & {- J_{{q/\omega},k}} & 0_{4 \times 3} & 0_{4 \times 1} \\J_{{v/q},k} & I_{3 \times 3} & 0_{3 \times 3} & {- C_{k}} & 0_{3 \times 1} \\0_{3 \times 4} & 0_{3 \times 3} & I_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 1} \\0_{3 \times 4} & 0_{3 \times 3} & 0_{3 \times 3} & I_{3 \times 3} & 0_{3 \times 1} \\0_{1 \times 1} & 0_{3 \times 3} & 0_{1 \times 3} & 0_{1 \times 3} & I_{1 \times 1}\end{bmatrix}} \\{W_{k} \approx \begin{bmatrix}J_{{q/\omega},k} & 0_{4 \times 3} & 0_{4 \times 3} & 0_{4 \times 3} & 0_{4 \times 1} \\0_{3 \times 3} & C_{k} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 1} \\0_{3 \times 3} & 0_{3 \times 3} & {\sqrt{\Delta \; t}I_{3 \times 3}} & 0_{3 \times 3} & 0_{3 \times 1} \\0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & {\sqrt{\Delta \; t}I_{3 \times 3}} & 0_{3 \times 1} \\0_{1 \times 3} & 0_{1 \times 3} & 0_{1 \times 3} & 0_{1 \times 3} & {\sqrt{\Delta \; t}I_{1 \times 1}}\end{bmatrix}} \\{\sum\limits_{\rho,k}^{2}{= \begin{bmatrix}\sum\limits_{{\eta \; \omega},k}^{2} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 1} \\0_{3 \times 3} & \sum\limits_{{\eta\alpha},k}^{2} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 1} \\0_{3 \times 3} & 0_{3 \times 3} & \sum\limits_{\rho\omega}^{2} & 0_{3 \times 3} & 0_{3 \times 1} \\0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & \sum\limits_{\rho \; \alpha}^{2} & 0_{3 \times 1} \\0_{1 \times 3} & 0_{1 \times 3} & 0_{1 \times 3} & 0_{1 \times 3} & {\sigma_{\rho \; g}^{2}I_{1 \times 1}}\end{bmatrix}}}\end{matrix} \right.}}}}}} & (20)\end{matrix}$

Here, 0_(n×m) indicates a zero matrix having n rows and m columns.I_(n×m) indicates an identity matrix having n rows and m columns. J . .. indicates a matrix of propagation coefficients of each error obtainedby partial differentiation of the process model, as with Expression(21). Σ . . . indicates a covariance matrix of each type of noise aswith Expression (22).

$\begin{matrix}{{J_{{q/q},k} = \begin{bmatrix}{{+ \Delta}\; q_{0,k}} & {{- \Delta}\; q_{1,k}} & {{- \Delta}\; q_{2,k}} & {{- \Delta}\; q_{3,k}} \\{{+ \Delta}\; q_{1,k}} & {{+ \Delta}\; q_{0,k}} & {{+ \Delta}\; q_{3,k}} & {{- \Delta}\; q_{2,k}} \\{{+ \Delta}\; q_{2,k}} & {{- \Delta}\; q_{3,k}} & {{+ \Delta}\; q_{0,k}} & {{+ \Delta}\; q_{1,k}} \\{{+ \Delta}\; q_{3,k}} & {{+ \Delta}\; q_{2,k}} & {{- \Delta}\; q_{1,k}} & {{+ \Delta}\; q_{0,k}}\end{bmatrix}}{J_{{q/\omega},k} = {{\frac{1}{2}\begin{bmatrix}{- q_{1,{k - 1}}} & {- q_{2,{k - 1}}} & {- q_{3,{k - 1}}} \\{+ q_{0,{k - 1}}} & {- q_{3,{k - 1}}} & {+ q_{2,{k - 1}}} \\{+ q_{3,{k - 1}}} & {+ q_{0,{k - 1}}} & {- q_{1,{k - 1}}} \\{- q_{2,{k - 1}}} & {+ q_{1,{k - 1}}} & {+ q_{0,{k - 1}}}\end{bmatrix}}\Delta \; t}}{{J_{{v/q},k} = {{2\begin{bmatrix}{+ r_{1,k}} & {+ r_{0,k}} & {+ r_{3,k}} & {- r_{2,k}} \\{+ r_{2,k}} & {- r_{3,k}} & {+ r_{0,k}} & {+ r_{1,k}} \\0 & 0 & 0 & 0\end{bmatrix}}\Delta \; t}},{\begin{bmatrix}r_{0,k} \\r_{1,k} \\r_{2,k} \\r_{3,k}\end{bmatrix} = {\begin{bmatrix}{+ q_{1,{k - 1}}} & {+ q_{2,{k - 1}}} & {+ q_{3,{k - 1}}} \\{+ q_{0,{k - 1}}} & {- q_{3,{k - 1}}} & {+ q_{2,{k - 1}}} \\{+ q_{3,{k - 1}}} & {+ q_{0,{k - 1}}} & {- q_{1,{k - 1}}} \\{- q_{2,{k - 1}}} & {+ q_{1,{k - 1}}} & {+ q_{0,{k - 1}}}\end{bmatrix}\lambda_{k}}}}} & (21) \\{{\sum\limits_{{\eta\omega},k}^{2}{= {{E\left\lbrack {\eta_{\omega}\eta_{\omega}^{T}} \right\rbrack} = \begin{bmatrix}\sigma_{{\eta \; \omega \; x},k}^{2} & 0 & 0 \\0 & \sigma_{{\eta \; \omega \; y},k}^{2} & 0 \\0 & 0 & \sigma_{{\eta \; \omega \; z},k}^{2}\end{bmatrix}}}}{\sum\limits_{{\eta \; \alpha},k}^{2}{= {{E\left\lbrack {\eta_{\alpha \;}\eta_{\alpha}^{T}} \right\rbrack} = \begin{bmatrix}\sigma_{{\eta \; \alpha \; x},k}^{2} & 0 & 0 \\0 & \sigma_{{\eta \; \alpha \; y},k}^{2} & 0 \\0 & 0 & \sigma_{{\eta \; \alpha \; z},k}^{2}\end{bmatrix}}}}{\sum\limits_{\rho \; \omega}^{2}{= {{E\left\lbrack {\rho_{\omega}\rho_{\omega}^{T}} \right\rbrack} = \begin{bmatrix}\sigma_{{{\rho\omega}\; x},k}^{2} & 0 & 0 \\0 & \sigma_{{{\rho\omega}\; y},k}^{2} & 0 \\0 & 0 & \sigma_{{{\rho\omega}\; z},k}^{2}\end{bmatrix}}}}{\sum\limits_{\rho\alpha}^{2}{= {{E\left\lbrack {\rho_{\alpha}\rho_{\alpha}^{T}} \right\rbrack} = \begin{bmatrix}\sigma_{{{\rho\alpha}\; x},k}^{2} & 0 & 0 \\0 & \sigma_{{{\rho\alpha}\; y},k}^{2} & 0 \\0 & 0 & \sigma_{{{\rho\alpha}\; z},k}^{2}\end{bmatrix}}}}} & (22)\end{matrix}$

Observation Model

In an observation model, an observation residual Δz in which anobservation residual Δz_(a) of the gravitational acceleration based onacceleration data d_(α) and an observation residual Δz_(v) of the zeromotion velocity are used as elements is calculated as in Expression(23).

$\begin{matrix}{{\Delta \; z_{k}} = {\begin{bmatrix}{\Delta \; z_{a,k}} \\{\Delta \; z_{v,k}}\end{bmatrix} = \begin{bmatrix}{{C_{k}{\overset{\_}{\alpha}}_{k}} - g_{k}} \\v_{k}\end{bmatrix}}} & (23)\end{matrix}$

Here, a Kalman coefficient K is calculated, as in Expression (24), byadding the noise component η_(α) of the acceleration data d_(α) and themotion acceleration component ζ_(a) and the motion velocity componentζ_(v) as observation errors.

$\begin{matrix}{{K_{k} = {\overset{2}{\sum\limits_{x,k}}{H_{k}^{T}\left( {{H_{k}{\sum\limits_{x,k}^{2}H_{k}^{T}}} + {V_{k}{\sum\limits_{\zeta,k}^{2}V_{k}^{T}}}} \right)}^{- 1}}}{H_{k} = \begin{bmatrix}J_{{{za}/q},k} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & J_{{{za}/g},k} \\0_{3 \times 4} & I_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 1}\end{bmatrix}}{\sum\limits_{\zeta,k}^{2}{= \begin{bmatrix}\sum\limits_{{\eta\alpha},k}^{2} & 0_{3 \times 3} & 0_{3 \times 3} \\0_{3 \times 3} & \sum\limits_{{\zeta \; a},k}^{2} & 0_{3 \times 3} \\0_{3 \times 3} & 0_{3 \times 3} & \sum\limits_{{\zeta \; v},k}^{2}\end{bmatrix}}}{V_{k} = \begin{bmatrix}C_{k} & {- I_{3 \times 3}} & 0_{3 \times 3} \\0_{3 \times 3} & 0_{3 \times 3} & {- I_{3 \times 3}}\end{bmatrix}}} & (24)\end{matrix}$

Here, J . . . is the matrix of propagation coefficients of each errorobtained by partial differentiation of the observation model, as withExpression (25). Σ . . . indicates a covariance matrix of each type ofnoise as with Expression (26)

$\begin{matrix}{{{J_{{{za}/q},k} = {2\begin{bmatrix}{+ s_{1,k}} & {+ s_{0,k}} & {+ s_{3,k}} & {- s_{2,k}} \\{+ s_{2,k}} & {- s_{3,k}} & {+ s_{0,k}} & {+ s_{1,k}} \\0 & 0 & 0 & 0\end{bmatrix}}},{\begin{bmatrix}s_{0,k} \\s_{1,k} \\s_{2,k} \\s_{3,k}\end{bmatrix} = {\begin{bmatrix}{+ q_{1,k}} & {+ q_{2,k}} & {+ q_{3,k}} \\{+ q_{0,k}} & {- q_{3,k}} & {+ q_{2,k}} \\{+ q_{3,k}} & {+ q_{0,k}} & {- q_{1,k}} \\{- q_{2,k}} & {+ q_{1,k}} & {+ q_{0,k}}\end{bmatrix}{\overset{\_}{\alpha}}_{k}}}}{J_{{{za}/g},k} = \begin{bmatrix}0 \\0 \\1\end{bmatrix}}} & (25) \\{{\sum\limits_{{\zeta \; a},k}^{2}{= {{E\left\lbrack {\zeta_{a}\zeta_{a}^{T}} \right\rbrack} = {\begin{bmatrix}\sigma_{{\zeta \; {ax}},k}^{2} & 0 & 0 \\0 & \sigma_{{\zeta \; {ay}},k}^{2} & 0 \\0 & 0 & \sigma_{{\zeta \; {az}},k}^{2}\end{bmatrix} + {\frac{\mu^{2}}{3}{{\Delta \; z_{a,k}}}^{2}I_{3 \times 3}}}}}}{\sum\limits_{{\zeta \; v},k}^{2}{= {{E\left\lbrack {\zeta_{v}\zeta_{v}^{T}} \right\rbrack} = \begin{bmatrix}\sigma_{{\zeta \; {vx}},k}^{2} & 0 & 0 \\0 & \sigma_{{\zeta \; {vy}},k}^{2} & 0 \\0 & 0 & \sigma_{{\zeta \; {vz}},k}^{2}\end{bmatrix}}}}} & (26)\end{matrix}$

Here, μ is a coefficient for calculating the RMS of the error of eachaxis from the predicted motion acceleration. The state vector x iscorrected, as in Expression (27), and the error covariance matrix Σ_(x)² thereof is updated, by using the Kalman coefficient K.

x _(k) ←x _(k) −K _(k)

z _(k)

Σ_(z,k) ²←Σ_(z,k) ² −K _(k) H _(k)Σ_(z,k) ²  (27)

Posture Normalization Model

In the posture normalization model, Expression (28) is updated in orderto maintain the posture quaternion and the error covariance thereof toproper values.

$\begin{matrix}{{x_{k} = \left. \begin{bmatrix}q_{k} \\v_{k} \\b_{\omega,k} \\b_{\alpha,k} \\{\Delta \; g_{k}}\end{bmatrix}\leftarrow\begin{bmatrix}\frac{q_{k}}{q_{k}} \\v_{k} \\b_{\omega,k} \\b_{\alpha,k} \\{\Delta \; g_{k}}\end{bmatrix} \right.}\left. \Sigma_{x,k}^{2}\leftarrow\begin{bmatrix}{D_{k}^{\prime \; T}D_{k}^{\prime}} & 0_{4 \times 10} \\0_{10 \times 4} & I_{10 \times 10}\end{bmatrix} \right.{\Sigma_{x,k}^{2}\begin{bmatrix}{D_{k}^{\prime \; T}D_{k}^{\prime}} & 0_{4 \times 10} \\0_{10 \times 4} & I_{10 \times 10}\end{bmatrix}}} & (28)\end{matrix}$

Here, D′^(T)D′ indicates a matrix of Expression (29) for limiting therank of the posture error and removing the azimuthal component.

$\begin{matrix}{{D_{k}^{'\; T}D_{k}^{\prime}} = \begin{bmatrix}{q_{1,k}^{2} + q_{2,k}^{2}} & {{{- q_{0,k}}q_{1,k}} - {q_{2,k}q_{3,k}}} & {{q_{1,k}q_{3,k}} - {q_{0,k}q_{2,k}}} & 0 \\{{{- q_{0,k}}q_{1,k}} - {q_{2,k}q_{3,k}}} & {q_{0,k}^{2} + q_{3,k}^{2}} & 0 & {{q_{0,k}q_{2,k}} - {q_{1,k}q_{3,k}}} \\{{q_{1,k}q_{3,k}} - {q_{0,k}q_{2,k}}} & 0 & {q_{0,k}^{2} + q_{3,k}^{2}} & {{{- q_{0,k}}q_{1,k}} - {q_{2,k}q_{3,k}}} \\0 & {{q_{0,k}q_{2,k}} - {q_{1,k}q_{3,k}}} & {{{- q_{0,k}}q_{1,k}} - {q_{2,k}q_{3,k}}} & {q_{1,k}^{2} + q_{2,k}^{2}}\end{bmatrix}} & (29)\end{matrix}$

1-1-8. Initial Value State Vector and Error Covariance Matrix

Initial values of the state vector x and the error covariance matrixΣ_(x) ² are given as in Expression (30).

$\begin{matrix}{{x_{0} = \begin{bmatrix}q_{0} \\v_{0} \\b_{\omega,0} \\b_{\alpha,0} \\{\Delta \; g_{0}}\end{bmatrix}}{\Sigma_{x,0}^{2} = \begin{bmatrix}\Sigma_{q,0}^{2} & 0_{4 \times 3} & 0_{4 \times 3} & 0_{4 \times 3} & 0_{4 \times 1} \\0_{3 \times 4} & \Sigma_{\upsilon,0}^{2} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 1} \\0_{3 \times 4} & 0_{3 \times 3} & \Sigma_{{b\; \omega},0}^{2} & 0_{3 \times 3} & 0_{3 \times 1} \\0_{3 \times 4} & 0_{3 \times 3} & 0_{3 \times 3} & \Sigma_{{b\; \alpha},0}^{2} & 0_{3 \times 1} \\0_{1 \times 4} & 0_{1 \times 3} & 0_{1 \times 3} & 0_{1 \times 1} & {\sigma_{{\Delta \; g},0}^{2}I_{1 \times 1}}\end{bmatrix}}} & (30)\end{matrix}$

Posture Quaternion

It is necessary that the posture of the IMU is given in quaternionexpression. The posture quaternion q can be calculated from a roll(bank) angle φ [rad], a pitch (elevation) angle θ [rad], and a yaw angle(azimuth) ψ [rad] used for the posture and the like of an aircraft, asin Expression (31).

$\begin{matrix}{q = {\begin{bmatrix}q_{0} \\q_{1} \\q_{2} \\q_{3}\end{bmatrix} = \begin{bmatrix}{{\sin \; \frac{\varphi}{2}\sin \; \frac{\theta}{2}\sin \; \frac{\psi}{2}} + {\cos \; \frac{\varphi}{2}\cos \; \frac{\theta}{2}\cos \; \frac{\psi}{2}}} \\{{\sin \; \frac{\varphi}{2}\cos \; \frac{\theta}{2}\cos \; \frac{\psi}{2}} - {\cos \; \frac{\varphi}{2}\sin \; \frac{\theta}{2}\sin \; \frac{\psi}{2}}} \\{{\cos \; \frac{\varphi}{2}\sin \; \frac{\theta}{2}\cos \; \frac{\psi}{2}} + {\sin \; \frac{\varphi}{2}\cos \; \frac{\theta}{2}\sin \; \frac{\psi}{2}}} \\{{\cos \; \frac{\varphi}{2}\cos \; \frac{\theta}{2}\sin \; \frac{\psi}{2}} - {\sin \; \frac{\varphi}{2}\sin \; \frac{\theta}{2}\cos \; \frac{\psi}{2}}}\end{bmatrix}}} & (31)\end{matrix}$

The error covariance matrix Σ_(q) ² is calculated from RMSσ_(ϕ) [radRMS] of a roll angle error and RMSσ_(θ) [rad RMS] of a pitch angleerror, as in Expression (32) (yaw angle error is ignored).

$\begin{matrix}{{\Sigma_{q}^{2} = {\frac{1}{4}{D^{\prime \; T}\begin{bmatrix}\sigma_{\varphi}^{2} & 0 \\0 & \sigma_{\theta}^{2}\end{bmatrix}}D^{\prime}}}{D^{\prime} = \begin{bmatrix}{- q_{1}} & {+ q_{0}} & {- q_{3}} & {+ q_{2}} \\{- q_{2}} & {+ q_{3}} & {+ q_{0}} & {- q_{1}}\end{bmatrix}}} & (32)\end{matrix}$

Motion Velocity Vector

If the initial state is stationary, the motion velocity vector v may beto be required to 0. The error covariance matrix Σ_(v) ² is given basedon RMSσ_(vx), RMSσ_(vy), and RMSσ_(vz) [Gs RMS] of the errors of themotion velocity vector v in the axes, regardless of being stationary, asin Expression (33)

$\begin{matrix}{\Sigma_{v}^{2} = \begin{bmatrix}\sigma_{vx}^{2} & 0 & 0 \\0 & \sigma_{vy}^{2} & 0 \\0 & 0 & \sigma_{vz}^{2}\end{bmatrix}} & (33)\end{matrix}$

Residual Biases of Angular Velocity/Acceleration Sensor

If the residual bias b_(ω) of the angular velocity sensor and theresidual bias b_(α) of the acceleration sensor are known, the residualbiases are required to be appropriately set. When the residual biasesare unknown, zero as an expected value is given to the residual biases.Error covariance matrixes Σ_(bω) ² and Σ_(bα) ² are given based on theerrors RMSσ_(bωx), RMSσ_(bωy), and RMSσ_(bωz) [rad/s RMS] of theresidual biases of the angular velocity sensor in the axes and theerrors RMSσ_(bαx), RMSσ_(bαy), and RMSσ_(bαz) [G RMS] of the residualbiases of the acceleration sensor in the axes, as in Expression (34).

$\begin{matrix}{{\Sigma_{b\; \omega}^{2} = \begin{bmatrix}\sigma_{b\; \omega \; x}^{2} & 0 & 0 \\0 & \sigma_{b\; \omega \; y}^{2} & 0 \\0 & 0 & \sigma_{b\; \omega \; z}^{2}\end{bmatrix}}{\Sigma_{b\; \alpha}^{2} = \begin{bmatrix}\sigma_{b\; \alpha \; x}^{2} & 0 & 0 \\0 & \sigma_{b\; \alpha \; y}^{2} & 0 \\0 & 0 & \sigma_{b\; \alpha \; z}^{2}\end{bmatrix}}} & (34)\end{matrix}$

Gravitational-Acceleration Correction Value

If the gravitational acceleration value is known, a difference from thestandard value 1 [G] (=9.80665 [m/s²]) is required to be appropriatelyset. When the gravitational acceleration value is unknown, zero as anexpected value is given to the gravitational acceleration value.RMSσ_(Δg), [G RMS] of the error of the gravitational acceleration valueis applied to the error covariance matrix.

1-1-9. Setting Value Sampling Interval

Since the posture detection processing from the output of the IMScorresponds to a time integration operation in principle, the samplinginterval Δt [s] is an important value, and thus is required to beappropriately set.

Output Noise of Angular Velocity/Acceleration Sensor

The noise components η included in outputs of the angular velocitysensor and the acceleration sensor are considered as the white Gaussiannoise of the variance σ_(η) ² which is average zero independent for eachaxis. The magnitude of the noise components is designated by thecorresponding RMS values (σ_(ηωx), σ_(ηωy), σ_(ηωz)) [rad/s RMS] and(σ_(nαx), σ_(ηαy), σ_(ηαz)) [G RMS], as in Expression (35).

$\begin{matrix}{{{\eta_{\omega} = {{\begin{bmatrix}\eta_{\omega \; x} \\\eta_{\omega \; y} \\\eta_{\omega \; z}\end{bmatrix} \approx \begin{bmatrix}{N\left( {0,\sigma_{\eta \; \omega \; x}^{2}} \right)} \\{N\left( {0,\sigma_{\eta \; \omega \; y}^{2}} \right)} \\{N\left( {0,\sigma_{{\eta\omega}\; z}^{2}} \right)}\end{bmatrix}} = {N\left( {0,\Sigma_{\eta \; \omega}^{2}} \right)}}},{\Sigma_{\eta \; \omega}^{2} = \begin{bmatrix}\sigma_{\eta \; \omega \; x}^{2} & 0 & 0 \\0 & \sigma_{\eta \; \omega \; y}^{2} & 0 \\0 & 0 & \sigma_{\eta \; \omega \; z}\end{bmatrix}}}{{\eta_{\alpha} = {{\begin{bmatrix}\eta_{\alpha \; x} \\\eta_{\alpha \; y} \\\eta_{\alpha \; z}\end{bmatrix} \approx \begin{bmatrix}{N\left( {0,\sigma_{\eta \; \alpha \; x}^{2}} \right)} \\{N\left( {0,\sigma_{\eta \; \alpha \; y}^{2}} \right)} \\{N\left( {0,\sigma_{\eta \; \alpha \; z}^{2}} \right)}\end{bmatrix}} = {N\left( {0,\Sigma_{\eta \; \alpha}^{2}} \right)}}},{\Sigma_{\eta\alpha}^{2} = \begin{bmatrix}\sigma_{\eta \; \alpha \; x}^{2} & 0 & 0 \\0 & \sigma_{\eta \; \alpha \; y}^{2} & 0 \\0 & 0 & \sigma_{\eta \; \alpha \; z}^{2}\end{bmatrix}}}} & (35)\end{matrix}$

Biases of Angular Velocity/Acceleration Sensor and Instability ofGravitational Acceleration Value

It is considered that the biases of the angular velocity sensor and theacceleration sensor are not constant and change with time. It isconsidered that the gravitational acceleration value also slightlychanges depending on the surrounding environment. Considering the changeas an individual random walk, the instability is designated by (σ_(ρωx),σ_(ρωy), σ_(ρωz)) [rad/s/√s], (σ_(ραx), σ_(ραy), σ_(ραz)) [G/√s], andσ_(ρg) [G/√s], as in Expression (36).

$\begin{matrix}{{{b_{\omega,k} = {b_{\omega,{k - 1}} + {N\left( {0,{\Sigma_{\rho\omega}^{2}\Delta \; t}} \right)}}},{\Sigma_{\rho\omega}^{2} = \begin{bmatrix}\sigma_{{\rho\omega}\; x}^{2} & 0 & 0 \\0 & \sigma_{{\rho\omega}\; y}^{2} & 0 \\0 & 0 & \sigma_{{\rho\omega}\; z}^{2}\end{bmatrix}}}{{b_{\alpha,k} = {b_{\alpha,{k - 1}} + {N\left( {0,{\Sigma_{\rho\alpha}^{2}\Delta \; t}} \right)}}},{\Sigma_{\rho \; \alpha}^{2} = \begin{bmatrix}\sigma_{\rho \; \alpha \; x}^{2} & 0 & 0 \\0 & \sigma_{\rho \; \alpha \; y}^{2} & 0 \\0 & 0 & \sigma_{\rho \; \alpha \; z}^{2}\end{bmatrix}}}{{\Delta \; g_{k}} = {{\Delta \; g_{k - 1}} + {N\left( {0,{\sigma_{\rho \; g}^{2}\Delta \; t}} \right)}}}} & (36)\end{matrix}$

Motion Acceleration

When the gravitational acceleration is observed in order to correct theposture, the motion acceleration component ζ_(a) acts as the observationerror. When this observation error is considered as simple whiteGaussian noise, a result that the posture sensitively responds to alarge motion acceleration due to a rapid motion is obtained. Thus, anoise model as with Expression (37) of changing the level depending onthe magnitude of the estimated motion acceleration (difference betweenthe observed acceleration and the estimated gravitational acceleration)is used, and a linear coefficient μ [N/A] and a constant term (σ_(ζαx),σ_(ζαy), σ_(ζαz)) [G RMS] are used as setting items.

$\begin{matrix}{{Ϛ_{a,k} \approx {N\left( {0,\Sigma_{{Ϛ\; a},k}^{2}} \right)}},{\Sigma_{{Ϛ\; a},k}^{2} = {\begin{bmatrix}\sigma_{Ϛ\; {ax}}^{2} & 0 & 0 \\0 & \sigma_{Ϛ\; {ay}} & 0 \\0 & 0 & \sigma_{Ϛ\; {az}}\end{bmatrix} + {\frac{\mu^{2}}{3}{{\Delta \; z_{a,k}}}^{2}I_{3 \times 3}}}}} & (37)\end{matrix}$

Motion Velocity

In observing the zero motion velocity, it is observed that the motionvelocity of the IMU is substantially zero in a long term. The motionvelocity component ζ_(v) appearing in a short term acts as theobservation error. This observation error is considered as the whiteGaussian noise independent for each axis, and the magnitude of thisobservation error is designated by the RMS values (σ_(ζvx), σ_(ζvy),σ_(ζvz)) [Gs RMS], as in Expression (38).

$\begin{matrix}{{Ϛ_{v} \approx {N\left( {0,\Sigma_{Ϛ\; v}^{2}} \right)}},{\Sigma_{Ϛ\; v}^{2} = \begin{bmatrix}\sigma_{Ϛ\; {vx}}^{2} & 0 & 0 \\0 & \sigma_{Ϛ\; {vy}}^{2} & 0 \\0 & 0 & \sigma_{Ϛ\; {vz}}^{2}\end{bmatrix}}} & (38)\end{matrix}$

1-1-10. Limitation of Yaw-Axis Component of Bias Error in AngularVelocity Sensor

When an azimuth observation section such as a magnetic sensor is notprovided, only a yaw-axis component (vertical component) of the biaserror of the angular velocity sensor also monotonously increases.Normally, a yaw-axis direction changes with the posture and the sensorcoordinates. Thus, for example, even though a z-axis of the angularvelocity sensor at a certain time point coincides with a yaw axis, andthus error estimate increases, correction is performed by observing thegravitational acceleration when the posture changes, and thus the z-axisbecomes an inclination (for example, a horizontal-axis direction) otherthan the yaw axis. Thus, the error estimate is reduced. However, forexample, when the posture changes small, and the substantially sameposture continues for a long time, the error estimate of the yaw-axiscomponent increases, and the feedback gain increases. In addition, theincrease of the feedback gain causes the bias estimation value of theangular velocity sensor to change without intention, and thus causes theazimuth of the posture to drift. In addition, it is unlikely that thebias estimation error of even the yaw-axis component of the practicalangular velocity sensor increases beyond, for example, the initial biaserror without limit. Accordingly, an upper limit value is provided onlyin the yaw-axis component of the bias estimation error of the angularvelocity sensor in the state error covariance matrix, and thus, theyaw-axis component is limited not to exceed the upper limit value.

Firstly, as in Expression (39), the variance σ_(bωv) ² of the yaw-axiscomponent is calculated from the error covariance matrix Σ_(bω) ² of theresidual bias in the angular velocity sensor, based on the currentposture quaternion.

$\begin{matrix}{{\sigma_{bwv}^{2} = {{c_{r}^{T}\Sigma_{bw}^{2}c_{v}} = {\begin{bmatrix}0_{7}^{T} & c_{v}^{T} & 0_{4}^{T}\end{bmatrix}{\Sigma_{x}^{2}\begin{bmatrix}0_{7} \\c_{v} \\0_{4}\end{bmatrix}}}}}\left\{ {c_{v} = \begin{bmatrix}{2\left( {{q_{1}q_{3}} - {q_{0}q_{2}}} \right)} \\{2\left( {{q_{2}q_{3}} + {q_{0}q_{1}}} \right)} \\{q_{0}^{2} - q_{1}^{2} - q_{2}^{2} + q_{3}^{2}}\end{bmatrix}} \right.} & (39)\end{matrix}$

When the variance σ_(bωv) ² of the yaw-axis component exceeds the upperlimit value σ_(bωmax) ², the yaw-axis component is limited as inExpression (40).

$\begin{matrix}{\mspace{79mu} {\left. \Sigma_{x}^{2}\leftarrow{L_{x}\Sigma_{x}^{2}L_{x}^{T}} \right.\left\{ {\begin{matrix}{L_{x} = \begin{bmatrix}I_{7 \times 7} & 0_{7 \times 3} & 0_{7 \times 4} \\0_{3 \times 7} & L_{bw} & 0_{3 \times 4} \\0_{4 \times 7} & 0_{4 \times 3} & I_{4 \times 4}\end{bmatrix}} \\{L_{bw} = {{C^{T}\begin{bmatrix}1 & 0 & 0 \\0 & 1 & 0 \\0 & 0 & \sqrt{\frac{\sigma_{\text{?}}^{2}}{\sigma_{bwr}^{2}}}\end{bmatrix}}C}} \\{C = \begin{bmatrix}{q_{0}^{2} + q_{1}^{2} - q_{2}^{2} - q_{3}^{2}} & {2\left( {{q_{1}q_{2}} - {q_{0}q_{3}}} \right)} & {2\left( {{q_{1}q_{3}} + {q_{0}q_{2}}} \right)} \\{2\left( {{q_{1}q_{2}} + {q_{0}q_{3}}} \right)} & {q_{0}^{2} - q_{1}^{2} + q_{2}^{2} - q_{3}^{2}} & {2\left( {{q_{2}q_{3}} - {q_{0}q_{1}}} \right)} \\{2\left( {{q_{1}q_{3}} - {q_{0}q_{2}}} \right)} & {2\left( {{q_{2}q_{3}} + {q_{0}q_{1}}} \right)} & {q_{0}^{2} - q_{1}^{2} - q_{2}^{2} + q_{3}^{2}}\end{bmatrix}}\end{matrix}\mspace{79mu} \text{?}\text{indicates text missing or illegible when filed}} \right.}} & (40)\end{matrix}$

1-1-11. Countermeasure for Off-Scale of Sensor

An effective measurement range is defined in an actual inertial sensor(angular velocity sensor and acceleration sensor). When a physicalquantity (angular velocity or acceleration) exceeding the range isinput, it is not possible to obtain an accurate output by saturation ofa signal in the inertial sensor. Exceeding the effective measurementrange is referred to as “off-scale” below. Normally, a sensor to be usedis selected to correspond to a range to be measured such that off-scaledoes not occur. However, a state (off-scale state) exceeding the rangeby an impact and the like in a very short time is assumed depending onthe application.

Influence of Sensor Off-Scale

If the angular velocity sensor is in the off-scale state, a large erroris included in the angular velocity, and an error in a posture angleobtained by integrating the angular velocity increases. An error in themotion velocity increases by integration of the acceleration based onthe posture angle having an increased error. If the acceleration sensoris in the off-scale state, a large error is included in theacceleration, and the error in the motion velocity obtained byintegrating the acceleration increases. Since the error by the off-scaleis not the stochastic process, obtaining the average or the variance isnot possible. Modeling of a Kalman filter is difficult because theamount of error is huge and unpredictable. Thus, in the current Kalmanfilter, the above error is not recognized as an error to be corrected.Thus, when the output value of the sensor is out of the effective rangeof the sensor, this situation is recognized as off-scale, and a properperiod after the time point at which the output value of the sensor isout of the effective range is defined as a recovery period from theoff-scale. Then, processing different from that in the normal time isperformed.

Off-Scale Recovery Processing

Following processing is applied to the state error covariance matrix tohandle the state error increasing by an influence of off-scale. That is,the error variance component of the covariance matrix is appropriatelyinflated in response to an increase of the error. The covariancecomponent is set to zero in order to separate the increased error fromother state variable errors. It is possible to strongly correct theincreased state variable of the error and to suppress the negativeinfluence on other state variables such as the bias estimation value tothe minimum, by performing the above processing before correctionprocessing.

Angular Velocity Off-Scale Recovery Processing

Processing of Expression (41) is applied in order to handle the postureangle and the error in motion velocity, which increase by the off-scalestate of the angular velocity sensor. That is, the error component(posture error component) of the posture quaternion q in the errorcovariance matrix Σ_(x) ² is appropriately increased in response to anincrease of the posture angle error. A correlation component (covariancecomponent) between the posture error component and an error componentother than the posture error component is set to zero in order toseparate the posture error component from other error components, in theerror covariance matrix Σ_(x) ². The error component other than theposture error component includes a motion velocity error component, anerror component of the residual bias b_(ω) of the angular velocitysensor (bias error component of angular velocity), an error component ofthe residual bias b_(α) of the acceleration sensor (bias error componentof acceleration), and an error component of thegravitational-acceleration correction value Δg. Setting the correlationcomponent (covariance component) to zero can include a value approximateto zero and a value allowing a predetermined error component to beseparated from other error components.

The error component of the motion velocity vector v (motion velocityerror component) in the error covariance matrix Σ_(x) ² is appropriatelyincreased in response to an increase of the motion velocity error. Acorrelation component (covariance component) between the motion velocityerror component and an error component other than the motion velocityerror component is set to zero in order to separate the motion velocityerror component from other error components, in the error covariancematrix Σ_(x) ². The error component other than the motion velocity errorcomponent includes the posture error component, the bias error componentof the angular velocity, the bias error component of the acceleration,and the error component of the gravitational-acceleration correctionvalue. In Expression (41), σ_(oq) ² indicates the variance of theposture error at time of off-scale recovery, and σ_(ov) ² indicates thevariance of the motion velocity error at the time of off-scale recovery.

$\begin{matrix}{{\left. \Sigma_{x}^{2}\leftarrow{{R_{w}\Sigma_{x}^{2}R_{w}^{T}} + \Sigma_{Rw}^{2}} \right. = \begin{bmatrix}{\frac{\sigma_{\text{?}}^{2}}{4}D^{\prime \; T}D^{\prime}} & 0_{4 \times 3} & 0_{4 \times 3} & 0_{4 \times 3} & 0_{4} \\0_{3 \times 4} & {\sigma_{ov}^{2}I_{3}} & 0_{3 \times 3} & 0_{3 \times} & 0_{3} \\0_{3 \times 4} & 0_{3 \times 3} & \Sigma_{w}^{2} & \Sigma_{w\; a} & \sigma_{wg} \\0_{3 \times 4} & 0_{3 \times 3} & \Sigma_{wa} & \Sigma_{a}^{2} & \sigma_{ag} \\0_{4}^{T} & 0_{3}^{T} & \sigma_{wg}^{T} & \sigma_{ag}^{T} & \sigma_{g}^{2}\end{bmatrix}}\left\{ {\begin{matrix}{R_{w} = \begin{bmatrix}0_{7 \times 7} & 0_{7 \times 7} \\0_{7 \times 7} & I_{7}\end{bmatrix}} \\{\Sigma_{Rw}^{2} = \begin{bmatrix}{\frac{\sigma_{\text{?}}^{2}}{4}D^{\prime \; T}D^{\prime}} & 0_{4 \times 3} & 0_{4 \times 7} \\0_{3 \times 4} & {\sigma_{ov}^{2}I_{3}} & 0_{3 \times 7} \\0_{7 \times 3} & 0_{7 \times 4} & 0_{7 \times 7}\end{bmatrix}} \\{\Sigma_{x}^{2} = \begin{bmatrix}\Sigma_{q}^{2} & \Sigma_{qv} & \Sigma_{qw} & \Sigma_{qa} & \sigma_{qg} \\\Sigma_{qv} & \Sigma_{v}^{2} & \Sigma_{rw} & \Sigma_{va} & \sigma_{rg} \\\Sigma_{qw} & \Sigma_{vw} & \Sigma_{w}^{2} & \Sigma_{wa} & \sigma_{wg} \\\Sigma_{qa} & \Sigma_{va} & \Sigma_{wa} & \Sigma_{a}^{2} & \sigma_{ag} \\\sigma_{qg}^{T} & \sigma_{vg}^{T} & \sigma_{wg}^{T} & \sigma_{ag}^{T} & \Sigma_{G}^{2}\end{bmatrix}} \\{{D^{\prime \; T}D^{\prime}} = \begin{bmatrix}{q_{1}^{2} + q_{2}^{2}} & {{{- q_{0}}q_{1}} - {q_{2}q_{3}}} & {{q_{1}q_{3}} - {q_{0}q_{2}}} & 0 \\{{{- q_{0}}q_{1}} - {q_{2}q_{3}}} & {q_{0}^{2} + q_{3}^{2}} & 0 & {{q_{1}q_{3}} - {q_{0}q_{2}}} \\{{q_{1}q_{3}} - {q_{0}q_{2}}} & 0 & {q_{0}^{2} + q_{3}^{2}} & {{{- q_{0}}q_{1}} - {q_{2}q_{3}}} \\0 & {{q_{1}q_{3}} - {q_{0}q_{2}}} & {{{- q_{0}}q_{1}} - {q_{2}q_{3}}} & {q_{1}^{2} + q_{2}^{2}}\end{bmatrix}}\end{matrix}\mspace{79mu} \text{?}\text{indicates text missing or illegible when filed}} \right.} & (41)\end{matrix}$

Acceleration Off-Scale Recovery Processing

Processing of Expression (42) is applied in order to handle the error inmotion velocity, which increases by the off-scale state of theacceleration sensor. That is, the motion velocity error component in theerror covariance matrix Σ_(x) ² is appropriately increased in responseto an increase of the motion velocity error. A correlation component(covariance component) between the motion velocity error component andan error component other than the motion velocity error component is setto zero in order to separate the motion velocity error component fromother state variable errors, in the error covariance matrix Σ_(x) ².

$\begin{matrix}{{\left. \Sigma_{x}^{2}\leftarrow{{R_{a}\Sigma_{x}^{2}R_{a}^{T}} + \Sigma_{Ra}^{2}} \right. = \begin{bmatrix}\Sigma_{q}^{2} & 0_{4 \times 3} & \Sigma_{qw} & \Sigma_{qa} & \sigma_{qg} \\0_{3 \times 4} & {\sigma_{ov}^{2}I_{3}} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3} \\\Sigma_{qw} & 0_{3 \times 3} & \Sigma_{w}^{2} & \Sigma_{wa} & \sigma_{wg} \\\Sigma_{qa} & 0_{3 \times 3} & \Sigma_{wa} & \Sigma_{a}^{2} & \sigma_{ag} \\\sigma_{qg}^{T} & 0_{3}^{T} & \sigma_{wg}^{T} & \sigma_{ag}^{T} & \sigma_{g}^{2}\end{bmatrix}}\left\{ \begin{matrix}{R_{a} = \begin{bmatrix}I_{4} & 0_{4 \times 3} & 0_{4 \times 7} \\0_{3 \times 4} & 0_{3 \times 3} & 0_{3 \times 7} \\0_{7 \times 4} & 0_{7 \times 3} & I_{7}\end{bmatrix}} \\{\Sigma_{Ra}^{2} = \begin{bmatrix}0_{4 \times 4} & 0_{4 \times 3} & 0_{4 \times 7} \\0_{3 \times 4} & {\sigma_{ov}^{2}I_{3}} & 0_{3 \times 7} \\0_{7 \times 4} & 0_{7 \times 3} & 0_{7 \times 7}\end{bmatrix}}\end{matrix} \right.} & (42)\end{matrix}$

1-2. Flowchart of Posture Estimation Method

FIG. 1 is a flowchart illustrating an example of procedures of a postureestimation method according to the embodiment. The procedures in FIG. 1are performed by a posture estimation device that estimates the postureof an object to which an IMU is attached, for example. The object is notparticularly limited, and for example, a vehicle, an electronic device,exercise equipment, a person, and an animal may be provided as theobject. The IMU may be detachable from the object. The IMU may beprovided in a state where the IMU is fixed to the object, and thusdetaching the IMU is not possible, for example, the IMU is mounted inthe object. For example, the posture estimation device may be a personalcomputer (PC) or may be various portable devices such as a smart phone.

As illustrated in FIG. 1, in the posture estimation method in theembodiment, firstly, the posture estimation device initializes the statevector x_(k) and the error covariance matrix Σ_(x, k) ² as errorinformation (initialization step S1). That is, the posture estimationdevice sets k=0 and sets (initializes) the state vector x₀ and the errorcovariance matrix Σ_(x, 0) ² at a time point to. Specifically, theposture estimation device sets the elements q₀, v₀, b_(ω, 0), b_(α, 0),and Δg₀ of the state vector x₀ and Σ_(q, 0) ², Σ_(v, 0) ², Σ_(bω, 0) ²,Σ_(bα, 0) ², and σ_(Δg, 0) ² included in the error covariance matrixΣ_(x, 0) ², which are expressed as in Expression (30).

For example, the posture estimation device may set the initial postureof the inertial measurement unit (IMU) to have a roll angle, a pitchangle, and a yaw angle which have been determined in advance, and mayset q₀ by substituting the roll angle, the pitch angle, and the yawangle into Expression (31). Alternatively, the posture estimation devicemay acquire acceleration data from the acceleration sensor in a statewhere the inertial measurement unit (IMU) is stopped. The postureestimation device may specify a direction of the gravitationalacceleration from the acceleration data, calculate a roll angle and apitch angle, and set the yaw angle to a predetermined value (forexample, 0). Then, the posture estimation device may set q₀ bysubstituting the roll angle, the pitch angle, and the yaw angle intoExpression (31). The inertial measurement unit (IMU) sets Σ_(q, 0) ² bysubstituting RMSσ_(ϕ) of a roll angle error and RMSσ_(θ) of a pitchangle error into Expression (32).

For example, the posture estimation device sets a state where theinertial measurement unit (IMU) is stopped to be an initial state andsets v₀ to 0. Then, the posture estimation device sets Σ_(v, 0) ² bysubstituting RMSσ_(vx), RMSσ_(vy), and RMSσ_(vz) of the error of themotion velocity vector v in the axes into Expression (33).

If the residual bias b_(ω) of the angular velocity sensor and theresidual bias b_(α) of the acceleration sensor are known, the postureestimation device sets the values of the residual biases to b_(ω, 0) andb_(α, 0). If b_(ω) and b_(α) are not known, the posture estimationdevice sets b_(ω, 0) and b_(α, 0) to zero. The posture estimation devicesets Σ_(bω, 0) ² and Σ_(bα, 0) ² by substituting the errors RMSσ_(bωx),RMSσ_(bωy), and RMSσ_(bωz) of the residual bias of the angular velocitysensor in the axes and the errors RMSσ_(bαx), RMSσ_(bαy), and RMSσ_(bαz)of the residual bias of the acceleration sensor in the axes intoExpression (34).

If the gravitational acceleration value is known, the posture estimationdevice sets the difference from 1 G to Δg₀. If the gravitationalacceleration value is not known, the posture estimation device sets Δg₀to zero. The posture estimation device sets RMSσ_(Δg) of the error ofthe gravitational acceleration value in σ_(Δg, 0) ².

Then, the posture estimation device acquires a measured value of theinertial measurement unit (IMU) (measured-value acquisition step S2).Specifically, the posture estimation device waits until the samplinginterval Δt elapses. If the sampling interval Δt elapses, the postureestimation device sets k=k+1 and t_(k)=t_(k−1)+Δt and acquires angularvelocity data d_(ω, k) and acceleration data d_(α, k) from the inertialmeasurement unit (IMU).

Then, the posture estimation device performs prediction processing (alsoreferred to as time update processing) of the state vector x_(k)(including the posture quaternion q_(k) being the posture information ata time point t_(k), as an element) and the error covariance matrixΣ_(x, k) ² being error information at the time point t_(k) (predictionstep S3).

FIG. 2 is a flowchart illustrating an example of procedures of the stepS3 in FIG. 1. As illustrated in FIG. 2, firstly, the posture estimationdevice performs processing of removing the residual biases b_(ω, k−1)and b_(α, k−1) which have been estimated at a time point t_(k−1), fromangular velocity data d_(ω, k) and acceleration data d_(α, k) at thetime point t_(k). The posture estimation device performs the aboveprocessing with Expression (19) (bias removal step S31).

Then, the posture estimation device performs processing(posture-change-amount calculation processing) of calculating theposture change amount Δq_(k) at the time point t_(k) based on the outputof the angular velocity sensor (posture-change-amount calculation stepS32). Specifically, the posture estimation device calculates the posturechange amount Δq_(k) with Expression (6), based on the angular velocitydata d_(ω, k).

The posture estimation device performs processing(velocity-change-amount calculation processing) of calculating thevelocity change amount (C_(k)λ_(k)−g_(k−1))Δt of the object based on theoutput of the acceleration sensor and the output of the angular velocitysensor (velocity-change-amount calculation step S33). Specifically, theposture estimation device calculates the velocity change amount(C_(k)λ_(k)−g_(k−1))Δt with Expressions (9), (10), and (12), based onthe acceleration data d_(α, k) and the angular velocity data d_(ω, k).

The posture estimation device performs processing (posture informationprediction processing) of predicting the posture quaternion q_(k) beingposture information of the object at the time point t_(k), by using theposture change amount Δq_(k) (posture information prediction step S34).In the posture information prediction step S34, the posture estimationdevice further performs processing (velocity information predictionprocessing) of predicting the motion velocity vector v_(k) beingvelocity information of the object at the time point t_(k) by using thevelocity change amount (C_(k)λ_(k)−g_(k−1))Δt. Specifically, in theposture information prediction step S34, the posture estimation deviceperforms processing of predicting the posture quaternion q_(k) and thestate vector x_(k) including the motion velocity vector v_(k) as theelement, by Expressions (6), (12), and (19).

Lastly, the posture estimation device performs processing of updatingthe error covariance matrix Σ_(x, k) ² at the time point t_(k) byExpressions (20) and (21) (error information update step S35).

Returning to FIG. 1, the posture estimation device performs processing(rotational error component removal processing) of removing a rotationalerror component around a reference vector in the error covariance matrixΣ_(x, k) ² being the error information (rotational error-componentremoval step S4). The reference vector is a vector observed by theobservation section. In the embodiment, the reference vector is agravitational acceleration vector observed by the acceleration sensorbeing the observation section. In the embodiment, a rotation erroraround the reference vector is an azimuth error. Thus, in the step S4,the posture estimation device performs processing of removing theazimuth error in the error covariance matrix Σ_(x, k) ². Specifically,the posture estimation device generates the error covariance matrixΣ_(x, k) ² in which the rank limitation and removal of the azimuth errorcomponent ε_(θz) in the error covariance matrix Σ_(q, k) ² of theposture quaternion q_(k) are performed, by Expressions (28) and (29).

Then, the posture estimation device performs off-scale recoveryprocessing (error information adjustment step S5). Specifically, in theerror information adjustment step S5, the posture estimation devicedetermines whether or not the output of the angular velocity sensor iswithin the effective range. When it is determined that the output of theangular velocity sensor is not within the effective range, the postureestimation device performs processing of increasing the posture errorcomponent in the error covariance matrix Σ_(x, k) ² being the errorinformation and reducing the correlation component between the postureerror component and the error component other than the posture errorcomponent, in the error covariance matrix Σ_(x, k) ² (error informationadjustment step S5). In the error information adjustment step S5, theposture estimation device determines whether or not the output of theacceleration sensor is within the effective range. When it is determinedthat the output of the angular velocity sensor or the output of theacceleration sensor is not within the corresponding effective range, theposture estimation device performs processing of increasing the motionvelocity error component in the error covariance matrix Σ_(x, k) ² beingthe error information and reducing the correlation component between themotion velocity error component and the error component other than themotion velocity error component in the error covariance matrix Σ_(x, k)².

FIG. 3 is a flowchart illustrating an example of procedures of the stepS5 in FIG. 1. As illustrated in FIG. 3, firstly, the posture estimationdevice determines whether or not the output (angular velocity datad_(ω)) of the angular velocity sensor is within the effective range(Step S51). When it is determined that the output of the angularvelocity sensor is within the effective range (Y in Step S51), theposture estimation device checks the remaining of an angular-velocityoff-scale recovery period (Step S52 a). When it is determined that theoutput of the angular velocity sensor is not within the effective range(N in Step S51), the posture estimation device starts or prolongs theangular-velocity off-scale recovery period (Step S52 b).

Then, the posture estimation device determines whether or not the output(acceleration data d_(α)) of the acceleration sensor is within theeffective range (Step S53). When the posture estimation devicedetermines that the output of the acceleration sensor is within theeffective range (Y in Step S53), the posture estimation device checksthe remaining of an acceleration off-scale recovery period (Step S54 a).When the posture estimation device determines that the output of theacceleration sensor is not within the effective range (N in Step S53),the posture estimation device starts or prolongs the accelerationoff-scale recovery period (Step S54 b).

Then, the posture estimation device determines whether or not thecurrent time is in the angular-velocity off-scale recovery period as afirst period after it is determined that the output of the angularvelocity sensor is not within the effective range (Step S55). When theposture estimation device determines that the current time is in theangular-velocity off-scale recovery period (Y in Step S55), the postureestimation device increases the posture error component in the errorcovariance matrix Σ_(x, k) ² being the error information (Step S56 a).The posture estimation device reduces the correlation component betweenthe posture error component and the error component other than theposture error component in the error covariance matrix Σ_(x, k) ², forexample, sets the correlation component to zero (Step S56 b). Theposture estimation device increases the motion velocity error componentin the error covariance matrix Σ_(x, k) ² (Step S58 a). The postureestimation device reduces the correlation component between the motionvelocity error component and the error component other than the motionvelocity error component in the error covariance matrix Σ_(x, k) ², forexample, sets the correlation component to zero (Step S58 b). Inpractice, the posture estimation device performs the processes of StepS56 a, Step S56 b, Step S58 a, and Step S58 b by updating the errorcovariance matrix Σ_(x, k) ² with Expression (41).

When the posture estimation device determines that the current time isnot in the angular-velocity off-scale recovery period (N in Step S55),the posture estimation device determines whether or not the current timeis in the acceleration off-scale recovery period as a second periodafter it is determined that the output of the acceleration sensor is notwithin the effective range (Step S57). When the posture estimationdevice determines that the current time is in the acceleration off-scalerecovery period (Y in Step S57), the posture estimation device increasesthe motion velocity error component in the error covariance matrixΣ_(x, k) ² (Step S58 a). The posture estimation device reduces thecorrelation component between the motion velocity error component andthe error component other than the motion velocity error component inthe error covariance matrix Σ_(x, k) ², for example, sets thecorrelation component to zero (Step S58 b). In practice, the postureestimation device performs the processes of Step S58 a and Step S58 b byupdating the error covariance matrix Σ_(x, k) ² with Expression (42).

When the posture estimation device determines that the current time isin neither the angular-velocity off-scale recovery period nor theacceleration off-scale recovery period (N in Step S55 and N in StepS57), the posture estimation device does not perform the processes ofStep S56 a, Step S56 b, Step S58 a, and Step S58 b.

Returning to FIG. 1, the posture estimation device performs processing(bias error limitation processing) of limiting the bias error componentof the angular velocity around the reference vector, in the errorcovariance matrix Σ_(x, k) ² being the error information (bias errorlimitation step S6). As described above, in the embodiment, thereference vector is the gravitational acceleration vector. Thus, in StepS6, the posture estimation device limits the bias error component of theangular velocity around the gravitational acceleration vector, that is,limits the vertical component (yaw-axis component) of the bias error ofthe angular velocity.

FIG. 4 is a flowchart illustrating an example of procedures of Step S6in FIG. 1. As illustrated in FIG. 4, firstly, the posture estimationdevice performs processing of calculating the variance σ_(bωv) ² of thevertical component of the bias error in the angular velocity withExpression (39) and the state vector x_(k) and the error covariancematrix Σ_(x, k) ² at the time point t_(k) (bias-error vertical componentcalculation step S61).

Then, the posture estimation device performs processing of determiningwhether or not the variance σ_(bωv) ² exceeds the upper limit value(bias error determination step S62).

When the variance σ_(bωv) ² exceeds the upper limit value (Y in StepS62), the posture estimation device performs limitation operationprocessing of limiting the vertical component of the bias error in theangular velocity (limitation operation step S63). Specifically, when thevariance σ_(bωv) ² exceeds the upper limit value σ_(bωmax) ², theposture estimation device updates the error covariance matrix Σ_(x, k) ²by Expression (40). When the variance σ_(bωv) ² is equal to or less thanthe upper limit value (N in Step S62), the posture estimation devicedoes not perform the limitation operation processing in Step S63.

Returning to FIG. 1, the posture estimation device performs correctionprocessing (also referred to as observation update processing) of thestate vector x_(k) and the error covariance matrix Σ_(x, k) ² at thetime point t_(k) (correction step S7). In the embodiment, the postureestimation device performs processing (posture information correctionstep) of correcting the posture quaternion q_(k) being the postureinformation of the object, which has been predicted in Step S3, based onthe error covariance matrix Σ_(x, k) ² being the error information.Specifically, in the posture information correction step, the postureestimation device corrects the state vector x_(k) including the posturequaternion q_(k) being the posture information as the element, based onthe observation residual Δz_(a, k) being a difference between theacceleration vector (obtained based on the output of the accelerationsensor) and the gravitational acceleration vector being the referencevector. FIG. 5 is a flowchart illustrating an example of procedures ofStep S7 in FIG. 1.

As illustrated in FIG. 5, firstly, the posture estimation deviceperforms processing of calculating the observation residual Δz_(k), aKalman coefficient K_(k), and a transformation matrix H_(k) at the timepoint t_(k) by Expressions (23) and (24) (correction coefficientcalculation step S71).

Then, the posture estimation device performs processing (postureinformation correction processing) of correcting the posture quaternionq_(k) being the predicted posture information of the object at the timepoint t_(k) (posture information correction step S72). Specifically, theposture estimation device performs processing of correcting the statevector x_(k) at the time point t_(k) with Expression (27), theobservation residual Δz_(k), and the Kalman coefficient K_(k).

The posture estimation device performs processing of normalizing thestate vector x_(k) at the time point t_(k) by Expression (28)(normalization step S73).

Then, the posture estimation device performs processing of correctingthe error covariance matrix Σ_(x, k) ² at the time point t_(k) withExpression (27), the Kalman coefficient K_(k), and the transformationmatrix H_(k) (error information correction step S74).

Returning to FIG. 1, the posture estimation device repeats the processesof Step S2 to Step S7 until an end instruction is received (N in StepS8). If the end instruction is received (Y in Step S8), the postureestimation device ends the processing.

The order of the steps in FIG. 1 can be appropriately changed. Forexample, the order of Step S4, Step S5, and Step S6 may be changed, andat least one of Step S4, Step S5, and Step S6 may be performed afterStep S7.

As described above, according to the posture estimation method in theembodiment, the posture change amount and the velocity change amount ofan object are calculated with Expressions (6) and (12) derived fromExpressions (1) and (2) of the output model of the IMU. Then, theposture of the object is estimated with the posture change amount andthe velocity change amount. In Expressions (6) and (12), a calculationerror in the posture change amount or the velocity change amount isreduced in comparison to that in the related art, by calculating theposture change amount and the velocity change amount with not only thefirst-order term of Δt but also the second-order term and thethird-order term thereof.

If the object rotates, the coordinate transformation matrix C_(k)changes. However, the coordinate transformation matrix C_(k) iscalculated from the elements of the posture quaternion q_(k) estimatedby the Kalman filter. Thus, when the object rotates rapidly, thecoordinate transformation matrix C_(k) may not immediately follow therotation of the object. Regarding this, according to the postureestimation method in the embodiment, since the acceleration λ_(k) iscalculated with not only the acceleration but also the angular velocityin Expression (12), the rotation of the object is immediately reflectedto the acceleration λ_(k). Thus, even when the object rotates rapidly,it is possible to reduce deterioration of calculation accuracy of thevelocity change amount.

Further, according to the posture estimation method in the embodiment,with Expression (23), the observation residual of the gravitationalacceleration by the output of the acceleration sensor and the motionvelocity of the object become zero in a long term, and the Kalmancoefficient K_(k) in Expression (24) is calculated with the observationresidual of the zero motion velocity. Thus, even though observationinformation of the azimuth is not provided, it is possible to estimatethe posture of the object with high accuracy.

In the posture estimation method in the embodiment, when the output ofthe angular velocity sensor is not within the effective range, theposture error component in the error covariance matrix Σ_(x, k) ² isincreased, and the correlation component between the posture errorcomponent and the error component other than the posture error componentis set to zero, by Expression (41). Accordingly, even when a situationin which an angular velocity exceeding the effective measurement rangeof the angular velocity sensor is temporarily input, and the output ofthe angular velocity sensor is temporarily not within the effectiverange occurs, it is possible to reduce a concern of decreasingestimation accuracy of the posture of the object.

In the posture estimation method in the embodiment, when the output ofthe angular velocity sensor is not within the effective range, themotion velocity error component in the error covariance matrix Σ_(x, k)² is increased, and the correlation component between the motionvelocity error component and the error component other than the motionvelocity error component is set to zero, by Expression (41). Inaddition, when the output of the acceleration sensor is not within theeffective range, the motion velocity error component in the errorcovariance matrix Σ_(x, k) ² is increased, and the correlation componentbetween the motion velocity error component and the error componentother than the motion velocity error component is set to zero, byExpression (42). Accordingly, even when a situation in which an angularvelocity exceeding the effective measurement range of the angularvelocity sensor or an acceleration exceeding the effective measurementrange of the acceleration sensor is temporarily input, and the output ofthe angular velocity sensor or the output of the acceleration sensor istemporarily not within the corresponding effective range occurs, it ispossible to reduce a concern of decreasing estimation accuracy of themotion velocity of the object.

Since information regarding the azimuth of the object is not included inthe output of the angular velocity sensor and the output of theacceleration sensor, the azimuth error of the object is not corrected.However, if the azimuth error included in the updated posture errorremains, reliability of the azimuth error is monotonously reduced, andthe posture error monotonously increases. Thus, the estimation accuracyof the posture may be deteriorated. Regarding this, according to theposture estimation method in the embodiment, the azimuth error includedin the posture error of the object, which has been updated by using theoutput of an angular velocity sensor 12 and the output of anacceleration sensor 14 is removed by Expression (17). Thus, it ispossible to reduce a concern of monotonously increasing the postureerror and to reduce a concern of decreasing the estimation accuracy ofthe posture.

The output of the angular velocity sensor and the output of theacceleration sensor do not include the information regarding the azimuthof the object. Thus, for example, when the object continues in thesubstantially same posture for a long term, the vertical component ofthe updated bias error of the angular velocity sensor monotonouslyincreases, and the Kalman coefficient K_(k) becomes too large.Consequently, there is a concern of deteriorating the estimationaccuracy of the posture. Regarding this, according to the postureestimation method in the embodiment, when the vertical component of thebias error in the angular velocity sensor exceeds the upper limit value,the vertical component of the bias error is limited to the upper limitvalue by Expression (40). Thus, it is possible to reduce a concern ofincreasing the Kalman coefficient K_(k) too large and to reduce aconcern of decreasing the estimation accuracy of the posture.

With the above descriptions, according to the posture estimation methodin the embodiment, it is possible to reduce a concern of decreasing theestimation accuracy of the posture of the object even when the postureof the object changes small and to estimate the posture of the objectwith sufficient accuracy.

2. Posture Estimation Device 2-1. Configuration of Posture EstimationDevice

FIG. 6 is a diagram illustrating an example of a configuration of theposture estimation device in the embodiment. As illustrated in FIG. 6,in the embodiment, a posture estimation device 1 includes a processingunit 20, a ROM 30, a RAM 40, a recording medium 50, and a communicationunit 60. The posture estimation device 1 estimates the posture of anobject based on an output of an inertial measurement unit (IMU) 10. Inthe embodiment, the posture estimation device 1 may have a configurationobtained by changing or removing some components or by adding anothercomponent.

As illustrated in FIG. 6, in the embodiment, the posture estimationdevice 1 is separated from the IMU 10. However, the posture estimationdevice 1 may include the IMU 10. The IMU 10 and the posture estimationdevice 1 may be accommodated in one casing. The IMU 10 may be separatedor separable from the main body in which the posture estimation device 1is accommodated. In the former case, the posture estimation device 1 ismounted on an object. In the latter case, the IMU 10 is mounted on theobject.

In the embodiment, the IMU 10 includes the angular velocity sensor 12,the acceleration sensor 14, and a signal processing unit 16. In theembodiment, the IMU 10 may have a configuration obtained by changing orremoving some components or by adding another component.

The angular velocity sensor 12 measures an angular velocity in each ofdirections of three axes which intersect with each other, in ideal, areperpendicular to each other. The angular velocity sensor 12 outputs ananalog signal depending on the magnitude and the orientation of themeasured three-axis angular velocity.

The acceleration sensor 14 measures an acceleration in each ofdirections of three axes which intersect with each other, in ideal, areperpendicular to each other. The acceleration sensor 14 outputs ananalog signal depending on the magnitude and the orientation of themeasured three-axis acceleration.

The signal processing unit 16 performs processing of performing samplingof an output signal of the angular velocity sensor 12 at a predeterminedsampling interval Δt to convert the output signal into angular velocitydata having a digital value. The signal processing unit 16 performsprocessing of performing sampling of an output signal of theacceleration sensor 14 at a predetermined sampling interval Δt toconvert the output signal into acceleration data having a digital value.

Ideally, the angular velocity sensor 12 and the acceleration sensor 14are attached to the IMU 10 such that the three axes coincide with threeaxes (x-axis, y-axis, and z-axis) of the sensor coordinate system whichis an orthogonal coordinate system defined for the IMU 10. However, inpractice, an error occurs in a mounting angle. Thus, the signalprocessing unit 16 performs processing of converting the angularvelocity data and the acceleration data into data in an xyz coordinatesystem, by using a correction parameter which has been, in advance,calculated in accordance with the error in the mounting angle. Thesignal processing unit 16 also performs processing of correcting thetemperature in the angular velocity data and the acceleration data inaccordance with temperature characteristics of the angular velocitysensor 12 and the acceleration sensor 14.

A function of A/D conversion or temperature correction may be embeddedin the angular velocity sensor 12 and the acceleration sensor 14.

The IMU 10 outputs angular velocity data d_(ω) and acceleration datad_(α) after the processing by the signal processing unit 16 to theprocessing unit 20 of the posture estimation device 1.

The ROM 30 stores programs used when the processing unit 20 performsvarious types of processing, and various programs or various types ofdata for realizing application functions, for example.

The RAM 40 is a storage unit that is used as a work area of theprocessing unit 20, and temporarily stores a program or data read outfrom the ROM 30 or operation results obtained by the processing unit 20performing processing in accordance with various programs, for example.

The recording medium 50 is a non-volatile storage unit that stores datarequired to be preserved for a long term among pieces of data generatedby processing of the processing unit 20. The recording medium 50 maystore programs used when the processing unit 20 performs various typesof processing, and various programs or various types of data forrealizing application functions, for example.

The processing unit 20 performs various types of processing inaccordance with the program stored in the ROM 30 or the recording medium50 or in accordance with the program which is received from a server viaa network and then is stored in the RAM 40 or the recording medium 50.In particular, in the embodiment, the processing unit 20 executes theprogram to function as a bias removal unit 22, a posture-change-amountcalculation unit 24, a velocity-change-amount calculation unit 26, and aposture estimation unit 28. Thus, the processing unit 20 performs apredetermined operation on the angular velocity data d_(ω) and theacceleration data d_(α) output at an interval of Δt by the IMU 10 so asto perform processing of estimating the posture of the object.

In the embodiment, as illustrated in FIG. 7, the sensor coordinatesystem (xyz coordinate system constituted by the x-axis, the y-axis, andthe z-axis which are perpendicular to each other) as the coordinatesystem of the IMU 10 and a local-space coordinate system (XYZ coordinatesystem constituted by an X-axis, a Y-axis, and a Z-axis which areperpendicular to each other) as a coordinate system of a space in whichthe object exists are considered. The processing unit 20 estimates theposture of the object (may also be referred to as the posture of the IMU10) in the local-space coordinate system from the three-axis angularvelocity and the three-axis acceleration in the sensor coordinatesystem, which are output from the IMU 10 mounted on the object.

The bias removal unit 22 performs processing of calculating thethree-axis angular velocity obtained by removing a bias error from theoutput of the angular velocity sensor 12 and performs processing ofcalculating the three-axis acceleration obtained by removing a biaserror from the output of the acceleration sensor 14.

The posture-change-amount calculation unit 24 calculates the posturechange amount of the object based on the output of the angular velocitysensor 12. Specifically, the posture-change-amount calculation unit 24performs processing of calculating the posture change amount of theobject by approximation with a polynomial expression in which thesampling interval Δt is used as a variable. The posture-change-amountcalculation unit 24 performs the processing with the three-axis angularvelocity in which the bias error has been removed by the bias removalunit 22.

The velocity-change-amount calculation unit 26 calculates the velocitychange amount of the object based on the output of the accelerationsensor 14 and the output of the angular velocity sensor 12.Specifically, the velocity-change-amount calculation unit 26 performsprocessing of calculating the velocity change amount of the object withthe three-axis angular velocity and the three-axis acceleration in whichthe bias error has been removed by the bias removal unit 22.

The posture estimation unit 28 functions as an integration calculationunit 101, a posture information prediction unit 102, an errorinformation update unit 103, a correction coefficient calculation unit104, a posture information correction unit 105, a normalization unit106, an error information correction unit 107, a rotationalerror-component removal unit 108, a bias error limitation unit 109, andan error information adjustment unit 110. The posture estimation unit 28performs processing of estimating the posture of the object with theposture change amount calculated by the posture-change-amountcalculation unit 24 and the velocity change amount calculated by thevelocity-change-amount calculation unit 26. In practice, the postureestimation unit 28 performs processing of estimating a state vector xdefined in Expression (18) and an error covariance matrix Σ_(x) ²thereof with an extended Kalman filter.

The integration calculation unit 101 performs integration processing ofintegrating the posture change amount calculated by theposture-change-amount calculation unit 24 with the previous estimatedvalue of the posture, which has been corrected by the postureinformation correction unit 105 and normalized by the normalization unit106. The integration calculation unit 101 performs integrationprocessing of integrating the velocity change amount calculated by thevelocity-change-amount calculation unit 26 with the previous estimatedvalue of the velocity, which has been corrected by the postureinformation correction unit 105 and normalized by the normalization unit106.

The posture information prediction unit 102 performs processing ofpredicting posture quaternion q as posture information of the object,with the posture change amount calculated by the posture-change-amountcalculation unit 24. The posture information prediction unit 102 alsoperforms processing of predicting a motion velocity vector v as velocityinformation of the object, based on the velocity change amountcalculated by the velocity-change-amount calculation unit 26. Inpractice, the posture information prediction unit 102 performsprocessing of predicting the state vector x including the posturequaternion q and the motion velocity vector v as elements.

The error information update unit 103 performs processing of updatingthe error covariance matrix Σ_(x) ² as error information, based on theoutput of the angular velocity sensor 12. Specifically, the errorinformation update unit 103 performs processing of updating a postureerror of the object with the three-axis angular velocity in which thebias error has been removed by the bias removal unit 22. In practice,the error information update unit 103 performs processing of updatingthe error covariance matrix Σ_(x) ² with the extended Kalman filter.

The rotational error-component removal unit 108 performs processing ofremoving a rotational error component around a reference vector, in theerror covariance matrix Σ_(x) ² being the error information.Specifically, the rotational error-component removal unit 108 performsprocessing of removing an azimuth error component included in theposture error in the error covariance matrix Σ_(x) ² updated by theerror information update unit 103. In practice, the rotationalerror-component removal unit 108 performs processing of generating theerror covariance matrix Σ_(x) ² in which the rank limitation and removalof the azimuth error component are performed in the error covariancematrix Σq² of the posture, on the error covariance matrix Σ_(x) ².

The error information adjustment unit 110 performs processing asfollows. That is, the error information adjustment unit 110 determineswhether or not the output of the angular velocity sensor 12 is within aneffective range. When the error information adjustment unit 110determines that the output of the angular velocity sensor 12 is notwithin the effective range, the error information adjustment unit 110increases a posture error component in the error covariance matrix Σ_(x)² being the error information and reduces a correlation componentbetween the posture error component and an error component other thanthe posture error component in the error covariance matrix Σ_(x) ² (forexample, sets the correlation component to zero). In addition, the errorinformation adjustment unit 110 performs processing as follows. That is,the error information adjustment unit 110 determines whether or not theoutput of the acceleration sensor 14 is within an effective range. Whenthe error information adjustment unit 110 determines that the output ofthe angular velocity sensor 12 or the output of the acceleration sensor14 is not within the corresponding effective range, the errorinformation adjustment unit 110 increases a motion velocity errorcomponent in the error covariance matrix Σ_(x, k) ² and reduces acorrelation component between the motion velocity error component and anerror component other than the motion velocity error component in theerror covariance matrix Σ_(x, k) ² (for example, sets the correlationcomponent to zero). Specifically, in an angular-velocity off-scalerecovery period after the error information adjustment unit 110determines that the output of the angular velocity sensor 12 is notwithin the effective range, the error information adjustment unit 110performs processing of increasing the posture error component and themotion velocity error component in the error covariance matrix Σ_(x) ²generated by the rotational error-component removal unit 108 andreducing the correlation component between the posture error componentand the error component other than the posture error component and thecorrelation component between the motion velocity error component andthe error component other than the motion velocity error component (forexample, setting the correlation components to zero). In an accelerationoff-scale recovery period after the error information adjustment unit110 determines that the output of the angular velocity sensor 12 iswithin the effective range, and the output of the acceleration sensor 14is not within the effective range, the error information adjustment unit110 performs processing of increasing a motion velocity error componentin the error covariance matrix Σ_(x) ² generated by the rotationalerror-component removal unit 108 and reducing a correlation componentbetween the motion velocity error component and an error component otherthan the motion velocity error component (for example, setting thecorrelation component to zero).

The bias error limitation unit 109 performs processing of limiting abias error component of an angular velocity around the reference vector,in the error covariance matrix Σ_(x) ² being the error information.Specifically, the bias error limitation unit 109 performs processing oflimiting a vertical component of the bias error of the angular velocity,in the error covariance matrix Σ_(x) ² generated by the errorinformation adjustment unit 110. In practice, the bias error limitationunit 109 performs processing as follows. That is, the bias errorlimitation unit 109 determines whether or not the vertical component ofthe bias error of the angular velocity exceeds an upper limit value.When the vertical component exceeds the upper limit value, the biaserror limitation unit 109 generates the error covariance matrix Σ_(x) ²in which limitation is applied such that the vertical component has theupper limit value.

The correction coefficient calculation unit 104 performs processing ofcalculating correction coefficients based on the error covariance matrixΣ_(x) ² which has been generated by the bias error limitation unit 109and is the error information. The correction coefficients are used fordetermining the correction amount of the posture information (posturequaternion q) or the velocity information (motion velocity vector v) ofthe object by the posture information correction unit 105 and thecorrection amount of the error information (error covariance matrix Σx)by the error information correction unit 107. In practice, thecorrection coefficient calculation unit 104 performs processing ofcalculating an observation residual Δz, a Kalman coefficient K, and atransformation matrix H.

The posture information correction unit 105 performs processing ofcorrecting the posture information (posture quaternion q) of the object,which has been predicted by the posture information prediction unit 102,based on the error covariance matrix Σx being the error information.Specifically, the posture information correction unit 105 performsprocessing of correcting the error covariance matrix Σx generated by thebias error limitation unit 109 and the posture quaternion q. The postureinformation correction unit 105 performs the processing with the Kalmancoefficient K and the observation residual Δz_(a) of the gravitationalacceleration calculated by the correction coefficient calculation unit104 based on the gravitational acceleration vector g being the referencevector and the acceleration vector α obtained from the output of theacceleration sensor 14. In practice, the posture information correctionunit 105 performs processing of correcting the state vector x predictedby the posture information prediction unit 102, with the extended Kalmanfilter.

The normalization unit 106 performs processing of normalizing theposture information (posture quaternion q) of the object, which has beencorrected by the posture information correction unit 105 such that themagnitude thereof does not change. In practice, the normalization unit106 performs processing of normalizing the state vector x corrected bythe posture information correction unit 105.

The error information correction unit 107 performs processing ofcorrecting the error covariance matrix Σx being the error information.Specifically, the error information correction unit 107 performsprocessing of correcting the error covariance matrix Σx generated by thebias error limitation unit 109 with the extended Kalman filter, and thetransformation matrix H and the Kalman coefficient K calculated by thecorrection coefficient calculation unit 104.

The posture information (posture quaternion q) of the object, which hasbeen estimated by the processing unit 20 can be transmitted to anotherdevice via the communication unit 60.

2-2. Configuration of Processing Unit

FIG. 8 is a diagram illustrating a specific example of a configurationof the processing unit 20. In FIG. 8, components as same as those inFIG. 6 are denoted by the same reference signs. As illustrated in FIG.8, angular velocity data d_(ω, k) and acceleration data d_(α, k) at atime point t_(k), which are output by the IMU 10 are input to the biasremoval unit 22. As shown in Expression (1), the angular velocity datad_(ω, k) is represented by the sum of the average value of the angularvelocity vector ω in a period from a time point t_(k−1) to the timepoint t_(k) and the residual bias b_(ω) of the angular velocity sensor12. Similarly, as shown in Expression (2), the acceleration datad_(α, k) is represented by the sum of the average value of theacceleration vector α in the period from the time point t_(k−1) to thetime point t_(k) and the residual bias b_(α) of the acceleration sensor14.

The bias removal unit 22 calculates the average value of the angularvelocity vector ω in the period from the time point t_(k−1) to the timepoint t_(k) by Expression (19) in a manner of subtracting the residualbias b_(ω, k−1) at the time point t_(k−1) from the angular velocity datad_(ω, k) at the time point t_(k). The bias removal unit 22 calculatesthe average value of the acceleration vector α in the period from thetime point t_(k−1) to the time point t_(k) by Expression (19) in amanner of subtracting the residual bias b_(α, k−1) at the time pointt_(k−1) from the acceleration data d_(α, k) at the time point t_(k).

The posture-change-amount calculation unit 24 calculates theapproximation of the posture change amount Δq_(k) at the time pointt_(k) by substituting the average value of the angular velocity vector ωin the period from the time point t_(k−1) to the time point t_(k) andthe average value of the angular velocity vector ω in a period from atime point t_(k−2) to the time point t_(k−1) into the polynomialexpression of Expression (6). The above-described average values of theangular velocity vector ω have been calculated by the bias removal unit22.

The velocity-change-amount calculation unit 26 calculates a coordinatetransformation matrix C_(k) at the time point t_(k) from posturequaternion q_(k−1) at the time point t_(k−1) with Expression (9). Thevelocity-change-amount calculation unit 26 calculates the approximationof the acceleration λ_(k) at the time point t_(k) by substituting theaverage value of the acceleration vector α and the average value of theangular velocity vector ω in the period from the time point t_(k−1) tothe time point t_(k), and the average value of the acceleration vector αand the average value of the angular velocity vector ω in the periodfrom the time point t_(k−2) to the time point t_(k−1) into thepolynomial expression of Expression (12). The above-described averagevalues of the angular velocity vector ω have been calculated by the biasremoval unit 22. The velocity-change-amount calculation unit 26calculates a gravitational acceleration vector g_(k−1) at the time pointt_(k−1) by substituting a gravitational-acceleration correction valueΔg_(k−1) at the time point t_(k−1) into Expression (10). Thevelocity-change-amount calculation unit 26 calculates the velocitychange amount (C_(k)λ_(k)−g_(k−1))Δt at the time point t_(k) from thecoordinate transformation matrix C_(k), the acceleration λ_(k), and thegravitational acceleration vector g_(k−1), which have been calculated.

As illustrated in FIG. 8, the posture estimation unit 28 includes theintegration calculation unit 101, the posture information predictionunit 102, the error information update unit 103, the correctioncoefficient calculation unit 104, the posture information correctionunit 105, the normalization unit 106, the error information correctionunit 107, the rotational error-component removal unit 108, and the biaserror limitation unit 109. The posture estimation unit 28 estimates thestate vector x and the error covariance matrix Σ_(x, k) ² at the timepoint t_(k) with the extended Kalman filter.

The integration calculation unit 101 performs quaternion multiplicationof the posture quaternion q_(k−1) at the time point t_(k−1) and theposture change amount Δq_(k) at the time point t_(k), which has beencalculated by the posture-change-amount calculation unit 24, withExpression (6). The integration calculation unit 101 adds a motionvelocity vector v_(k−1) at the time point t_(k−1) and the velocitychange amount (C_(k)λ_(k)−g_(k−1))Δt at the time point t_(k), which hasbeen calculated by the velocity-change-amount calculation unit 26, withExpression (12).

The posture information prediction unit 102 predicts the posturequaternion q_(k), the motion velocity vector v_(k), the residual biasb_(ω, k) of the angular velocity sensor 12, the residual bias b_(α, k)of the acceleration sensor 14, and the gravitational-accelerationcorrection value Δg_(k) which are elements of the state vector x_(k),with Expression (19). Specifically, the posture information predictionunit 102 predicts the posture quaternion q_(k) as a result of thequaternion multiplication of the posture quaternion q_(k−1) and theposture change amount Δq_(k) by the integration calculation unit 101.The posture information prediction unit 102 predicts the motion velocityvector v_(k) as a result of adding the motion velocity vector v_(k−1)and the velocity change amount (C_(k)λ_(k)−g_(k−1))Δt by the integrationcalculation unit 101. The posture information prediction unit 102predicts the residual bias b_(ω, k) Of the angular velocity sensor 12 tobe the residual bias b_(ω, k−1) of the angular velocity sensor 12 at thetime point t_(k−1). The posture information prediction unit 102 predictsthe residual bias b_(α, k) of the acceleration sensor 14 to be theresidual bias b_(α, k−1) of the acceleration sensor 14 at the time pointt_(k−1). The posture information prediction unit 102 predicts thegravitational-acceleration correction value Δg_(k) to be thegravitational-acceleration correction value Δg_(k−1) at the time pointt_(k−1).

The error information update unit 103 updates the error covariancematrix Σ_(x, k) ² at the time point t_(k) with Expressions (20) and(21). The error information update unit 103 performs the update with theposture change amount Δq_(k) calculated by the posture-change-amountcalculation unit 24, the acceleration λ_(k) and the coordinatetransformation matrix C_(k) calculated by the velocity-change-amountcalculation unit 26, the posture quaternion q_(k−1) at the time pointt_(k−1), and the error covariance matrix Σ_(x, k) ² at the time pointt_(k−1).

The rotational error-component removal unit 108 calculates a matrixD′^(T)D′ with the posture quaternion q_(k) predicted by the postureinformation prediction unit 102, by Expression (29). The rotationalerror-component removal unit 108 updates the error covariance matrixΣ_(x, k) ² updated by the error information update unit 103, withExpression (28) and the matrix D′^(T)D′. Thus, the error covariancematrix Σ_(x, k) ² in which the rank of the error covariance matrixΣ_(q, k) ² Of the posture quaternion q_(k) is limited to 3, and anazimuth error component ε_(θz) has been removed from the errorcovariance matrix Σ_(q, k) ² is generated.

In the angular-velocity off-scale recovery period, the error informationadjustment unit 110 updates the error covariance matrix Σ_(x, k) ²generated by the rotational error-component removal unit 108, withExpression (41) and the posture quaternion q_(k) predicted by theposture information prediction unit 102. In the acceleration off-scalerecovery period, the error information adjustment unit 110 updates theerror covariance matrix Σ_(x, k) ² generated by the rotationalerror-component removal unit 108 with Expression (42) and the posturequaternion q_(k) predicted by the posture information prediction unit102.

The bias error limitation unit 109 calculates the variance σ_(bωv) ² ofthe vertical component of the bias error in the angular velocity sensor12 with the posture quaternion q_(k) predicted by the postureinformation prediction unit 102 and the error covariance matrix Σ_(x, k)² generated by the error information adjustment unit 110, by Expression(39). When the variance σ_(bωv) ² exceeds the upper limit valueσ_(bωmax) ², the bias error limitation unit 109 updates the errorcovariance matrix Σ_(x, k) ² generated by the error informationadjustment unit 110, with Expression (40). Thus, in the error covariancematrix Σ_(x, k) ², the variance σ_(bωv) ² of the vertical component ofthe bias error in the angular velocity sensor 12 is limited to the upperlimit value σ_(bωmax) ².

The correction coefficient calculation unit 104 calculates theobservation residual Δz_(k), the transformation matrix H_(k), and theKalman coefficient K_(k) at the time point t_(k) with Expressions (23)to (26). The correction coefficient calculation unit 104 performs thecalculation with the error covariance matrix Σ_(x, k) ² generated by thebias error limitation unit 109, the coordinate transformation matrixC_(k) calculated by the velocity-change-amount calculation unit 26, theaverage value of the acceleration vector α calculated by the biasremoval unit 22 in the period from the time point t_(k−1) to the timepoint t_(k), and the posture quaternion q_(k) and thegravitational-acceleration correction value Δg_(k) predicted by theposture information prediction unit.

The posture information correction unit 105 corrects the elements(posture quaternion q_(k), motion velocity vector v_(k), residual biasb_(ω, k) of the angular velocity sensor 12, residual bias b_(α, k) ofthe acceleration sensor 14, and gravitational-acceleration correctionvalue Δg_(k)) of the state vector x_(k) predicted by the postureinformation prediction unit 102. The posture information correction unit105 performs the correction by Expression (27) with the observationresidual Δz_(k) and the Kalman coefficient K_(k) calculated by thecorrection coefficient calculation unit 104.

The normalization unit 106 normalizes the elements (posture quaternionq_(k), motion velocity vector v_(k), residual bias b_(ω, k) of theangular velocity sensor 12, residual bias b_(α, k) of the accelerationsensor 14, and gravitational-acceleration correction value Δg_(k)) ofthe state vector x_(k) corrected by the posture information correctionunit 105, with Expression (28).

The error information correction unit 107 corrects the error covariancematrix Σ_(x, k) ² generated by the bias error limitation unit 109 byExpression (27) with the transformation matrix H_(k) and the Kalmancoefficient K_(k) calculated by the correction coefficient calculationunit 104.

If the next sampling interval Δt has elapsed, the state vector x_(k)calculated by the normalization unit 106 and the error covariance matrixΣ_(x, k) ² corrected by the error information correction unit 107 arefed back to the bias removal unit 22, the velocity-change-amountcalculation unit 26, the integration calculation unit 101, the postureinformation prediction unit 102, and the error information update unit103 in a state of being set to be the state vector x_(k−1) and the errorcovariance matrix Σ_(x, k−1) ² at the time point t_(k−1).

The processing unit 20 described above performs posture estimationprocessing of estimating the posture of the object, for example, inaccordance with the procedures illustrated in FIGS. 1 to 5.

According to the above-described posture estimation device 1 in theembodiment, the processing unit 20 estimates the posture of an object inaccordance with the procedures illustrated in FIGS. 1 to 5. Thus, evenwhen a situation in which the output of the angular velocity sensor istemporarily not within the effective range occurs, it is possible toreduce a concern of decreasing the estimation accuracy of the posture ofthe object and to estimate the posture of the object with sufficientaccuracy. According to the posture estimation device 1 in theembodiment, even when a situation in which the output of the angularvelocity sensor or the output of the acceleration sensor is temporarilynot within the corresponding effective range occurs, it is possible toreduce a concern of decreasing the estimation accuracy of the motionvelocity of the object and to estimate the motion velocity of the objectwith sufficient accuracy. In addition, according to the postureestimation device 1 in the embodiment, it is possible to exhibit effectssimilar to those in the above-described posture estimation method in theembodiment.

3. Modification Examples

In the above-described embodiment, the angular velocity sensor and theacceleration sensor are integrated by being accommodated in one inertialmeasurement unit (IMU). However, the angular velocity sensor and theacceleration sensor may be provided to be individual from each other.

In the above-described embodiment, the posture estimation device outputsonly posture information of the object. However, the posture estimationdevice may output another kind of information. For example, the postureestimation device may output position information of the object, whichis obtained by integrating the velocity information or the motionvelocity vector v_(k) of the object based on the motion velocity vectorv_(k) at the time point t_(k).

In the above-described embodiment, the acceleration sensor and theangular velocity sensor update the outputs at the same sampling intervalΔt. However, the sampling interval Δt_(a) of the acceleration sensor maybe different from the sampling interval Δt_(ω) of the angular velocitysensor. In this case, the prediction processing (time update processing)of the state vector and the error covariance matrix in Step S3 in FIG. 1may be performed for each Δt_(ω), and correction processing (observationupdate processing) of the state vector and the error covariance matrixin Step S7 in FIG. 1 may be performed for each Δt_(a). The rotationalerror-component removal processing in Step S4 in FIG. 1 and the biaserror limitation processing in Step S6 in FIG. 1 may be performed foreach Δt_(ω), for each Δt_(a), or performed in a period different fromΔt_(ω) or Δt_(a).

In the above-described embodiment, the posture estimation deviceestimates the posture of an object by using the gravitationalacceleration vector observed by the acceleration sensor as the referencevector, and by using the output of the angular velocity sensor and thereference vector. However, the reference vector may not be thegravitational acceleration vector. For example, when the postureestimation device estimates the posture of an object by using theangular velocity sensor and a geomagnetic sensor, the reference vectormay be a geomagnetic vector (vector directed toward the north) observedby using a geomagnetic sensor. For example, when the posture estimationdevice estimates the posture of a satellite as an object by the angularvelocity sensor and a star tracker, the reference vector may be a vectorwhich is directed from the object toward a fixed star and is observed bythe star tracker. The acceleration sensor, the geomagnetic sensor, andthe star tracker are examples of a reference observation sensor thatobserves the reference vector.

In the above-described embodiment, the posture information of an objectis expressed in quaternion. However, the posture information may beinformation expressed by a roll angle, a pitch angle, and a yaw angle ormay be a posture transformation matrix.

In the above-described embodiment, the state vector x_(k) includes theposture quaternion q_(k), the motion velocity vector v_(k), the residualbias b_(ω, k) of the angular velocity sensor, the residual bias b_(α, k)of the acceleration sensor, and the gravitational-accelerationcorrection value Δg_(k), as the elements. However, the state vectorx_(k) is not limited thereto. For example, the state vector x_(k) mayinclude the posture quaternion q_(k), the residual bias b_(ω, k) of theangular velocity sensor, the residual bias b_(α, k) of the accelerationsensor, and the gravitational-acceleration correction value Δg_(k), asthe elements, and may not include the motion velocity vector v_(k). Inthis case, the flowchart of the bias error limitation step illustratedin FIG. 3 is changed to the flowchart illustrated in FIG. 9, forexample. In FIG. 9, steps as same as those in FIG. 3 are denoted by thesame reference signs. The flowchart in FIG. 9 is obtained by removingsome steps from the flowchart in FIG. 3, not by adding a new step. Thus,descriptions thereof will not be repeated.

4. Electronic Device

FIG. 10 is a block diagram illustrating an example of a configuration ofan electronic device 300 in the embodiment. The electronic device 300includes the posture estimation device 1 and the inertial measurementunit 10 in the above embodiment. The electronic device 300 can include acommunication unit 310, an operation unit 330, a display unit 340, astorage unit 350, and an antenna 312.

The communication unit 310 is, for example, a wireless circuit. Thecommunication unit 310 performs processing of receiving data from theoutside of the device or transmitting data to the outside thereof, viathe antenna 312.

The posture estimation device 1 performs processing based on the outputsignal of the inertial measurement unit 10. Specifically, the postureestimation device 1 performs processing of estimating the posture of theelectronic device 300 based on an output signal (output data) such asdetection data of the inertial measurement unit 10. The postureestimation device 1 may perform signal processing such as correctionprocessing or filtering on the output signal (output data) such asdetection data of the inertial measurement unit 10. In addition, basedon the output signals, the posture estimation device 1 may performvarious types of control processing for the electronic device 300, suchas control processing of the electronic device 300 or various types ofdigital processing of data transmitted or received via the communicationunit 310. The function of the posture estimation device 1 can berealized by a processor such as an MPU or a CPU, for example.

The operation unit 330 is used when a user performs an input operation.The operation unit 330 can be realized by an operation button, a touchpanel display, or the like.

The display unit 340 displays various types of information and can berealized by a display of liquid crystal, organic EL, or the like. Thestorage unit 350 stores data. The function of the storage unit 350 canbe realized by, for example, a semiconductor memory such as a RAM or aROM.

The electronic device 300 in the embodiment can be applied to, forexample, an image-related device (such as a digital still camera or avideo camera), an in-vehicle device, a wearable device (such as a headmounted display device or a watch-related device), an ink jet dischargedevice, a robot, a personal computer, a portable information terminal, aprinting device, and a projection device. The in-vehicle device includesa car navigation device or a device for automatic driving, for example.The watch-related device includes a watch or a smart watch, for example.For example, an ink jet printer is provided as the ink jet dischargedevice. The portable information terminal includes a smart phone, aportable phone, a portable game device, a notebook PC, or a tabletterminal, for example. The electronic device 300 in the embodiment canalso be applied to an electronic notebook, an electronic dictionary, acalculator, a word processor, a workstation, a videophone, a televisionmonitor for crime prevention, electronic binoculars, a POS terminal, amedical device, a fish finder, a measuring device, and a device for abase station of a mobile terminal, instruments, a flight simulator, anda network server, for example. The medical device includes an electronicthermometer, a sphygmomanometer, a blood glucose meter, anelectrocardiogram measuring device, an ultrasonic diagnostic device, anelectronic endoscope, and the like. The instruments are instruments ofvehicles, aircraft, ships, and the like.

FIG. 11 is a plan view illustrating a wrist-watch type activity meter400 as a portable electronic device. FIG. 12 is a block diagramillustrating an example of a configuration of the activity meter 400.The activity meter 400 is put on the region (such as the wrist) of auser by a band 401. The activity meter 400 which is an active trackerincludes a display unit 402 for digital display. The activity meter 400can perform wireless communication by Bluetooth (registered trademark),Wi-Fi (registered trademark), or the like.

As illustrated in FIGS. 11 and 12, the activity meter 400 includes acase 403, the posture estimation device 1, the display unit 402, and atranslucent cover 404. In the case 403, the inertial measurement unit 10is accommodated. The posture estimation device 1 is accommodated in thecase 403 and performs processing based on an output signal from theinertial measurement unit 10. The display unit 402 is accommodated inthe case 403. The translucent cover 404 closes an opening portion of thecase 403. A bezel 405 is provided on the outside of the translucentcover 404. A plurality of operation buttons 406 and 407 are provided ona side surface of the case 403. The acceleration sensor 14 that detectsa three-axis acceleration and the angular velocity sensor 12 thatdetects a three-axis angular velocity are provided in the inertialmeasurement unit 10.

In the display unit 402, position information or the movement amountobtained by a GPS sensor 411 or a geomagnetic sensor 412, motioninformation (such as a momentum) obtained by the acceleration sensor 14or the angular velocity sensor 12, biometric information (such as apulse rate) obtained by a pulse rate sensor 416, and time pointinformation such as the current time point are displayed in accordancewith various detection modes. An environmental temperature obtained by atemperature sensor 417 can also be displayed. A communication unit 422communicates with an information terminal such as a user terminal. Theposture estimation device 1 is realized by an MPU, a DSP, and an ASIC,for example. The posture estimation device 1 performs various types ofprocessing based on programs stored in a storage unit 420 andinformation input by an operation unit 418 such as the operation buttons406 and 407. The posture estimation device 1 performs processing ofestimating posture information of the activity meter 400 based on theoutput signal of the inertial measurement unit 10. The postureestimation device 1 may perform processing based on output signals ofthe GPS sensor 411, the geomagnetic sensor 412, a pressure sensor 413,the acceleration sensor 14, the angular velocity sensor 12, the pulserate sensor 416, the temperature sensor 417, and a timekeeping unit 419.The posture estimation device 1 can also perform display processing ofdisplaying an image in the display unit 402, sound output processing ofoutputting sound to a sound output unit 421, communication processing ofcommunicating with an information terminal via the communication unit422, power control processing of supplying power from a battery 423 tothe components, and the like.

According to the activity meter 400 having the above-describedconfiguration in the embodiment, it is possible to exhibit theabove-described effects of the posture estimation device 1 and toexhibit high reliability. The activity meter 400 includes the GPS sensor411 and can measure the movement distance and a movement trajectory ofthe user. Thus, the activity meter 400 having high usability isobtained. The activity meter 400 can be widely applied to a runningwatch, a runner watch, an outdoor watch, a GPS watch equipped with aGPS, and the like.

5. Vehicle

In the embodiment, a vehicle includes the posture estimation device 1 inthe above embodiment, and a control device that controls the posture ofthe vehicle based on the posture information of the vehicle, which hasbeen estimated by the posture estimation device 1.

FIG. 13 illustrates an example of a vehicle 500. FIG. 14 is a blockdiagram illustrating an example of a configuration of the vehicle 500.As illustrated in FIG. 13, the vehicle 500 includes a vehicle body 502and wheels 504. A positioning device 510 is mounted on the vehicle 500.A control device 570 that performs vehicle control and the like isprovided in the vehicle 500. As illustrated in FIG. 14, the vehicle 500includes a driving mechanism 580 such as an engine and a motor, abraking mechanism 582 such as a disk brake and a drum brake, and asteering mechanism 584 realized by a steering wheel, a steering gear boxand the like. As described above, the vehicle 500 is equipment or adevice that includes the driving mechanism 580, the braking mechanism582, and the steering mechanism 584 and moves on the ground, in the sky,or in the sea. For example, the vehicle 500 is a four-wheel vehicle suchas an agricultural machine.

The positioning device 510 is a device that is mounted on the vehicle500 and performs positioning of the vehicle 500. The positioning device510 includes the inertial measurement unit 10, a GPS receiving unit 520,an antenna 522 for GPS reception, and the posture estimation device 1.The posture estimation device 1 includes a position informationacquisition unit 532, a position composition unit 534, an operationalprocessing unit 536, and a processing unit 538. The inertial measurementunit 10 includes a three-axis acceleration sensor and a three-axisangular velocity sensor. The operational processing unit 536 receivesacceleration data and angular velocity data from the acceleration sensorand the angular velocity sensor, performs inertial navigationoperational processing on the received data, and outputs inertialnavigation positioning data. The inertial navigation positioning dataindicates the acceleration or the posture of the vehicle 500.

The GPS receiving unit 520 receives a signal from a GPS satellite viathe antenna 522. The position information acquisition unit 532 outputsGPS positioning data based on a signal received by the GPS receivingunit 520. The GPS positioning data indicates the position, the speed,and the direction of the vehicle 500 on which the positioning device 510is mounted. The position composition unit 534 calculates a position atwhich the vehicle 500 runs on the ground at the current time, based onthe inertial navigation positioning data output from the operationalprocessing unit 536 and the GPS positioning data output from theposition information acquisition unit 532. For example, if the postureof the vehicle 500 differs by an influence of an inclination (θ) of theground and the like as illustrated in FIG. 13 even though the positionof the vehicle 500, which is included in the GPS positioning data is thesame, the vehicle 500 runs at a different position on the ground.Therefore, it is not possible to calculate the accurate position of thevehicle 500 only with the GPS positioning data. Thus, the positioncomposition unit 534 calculates a position at which the vehicle 500 runson the ground at the current time, by using data regarding the postureof the vehicle 500 among types of inertial navigation positioning data.Position data output from the position composition unit 534 is subjectedto predetermined processing by the processing unit 538, and is displayedin a display unit 550, as a positioning result. The position data may betransmitted to an external device by a communication unit 560.

The control device 570 controls the driving mechanism 580, the brakingmechanism 582, and the steering mechanism 584 of the vehicle 500. Thecontrol device 570 is a controller of controlling the vehicle. Forexample, the control device 570 can be realized by a plurality ofcontrol units. The control device 570 includes a vehicle control unit572 being a control unit that controls the vehicle, an automatic drivingcontrol unit 574 being a control unit that controls automatic driving,and a storage unit 576 realized by a semiconductor memory and the like.A monitoring device 578 monitors an object such as an obstacle aroundthe vehicle 500. The monitoring device 578 is realized by a surroundingmonitoring camera, a millimeter wave radar, a sonar or the like.

As illustrated in FIG. 14, the vehicle 500 in the embodiment includesthe posture estimation device 1 and the control device 570. The controldevice 570 controls the posture of the vehicle 500 based on postureinformation of the vehicle 500, which has been estimated by the postureestimation device 1. For example, the posture estimation device 1performs various types of processing as described above, based on anoutput signal including detection data from the inertial measurementunit 10, so as to obtain information of the position or the posture ofthe vehicle 500. For example, the posture estimation device 1 can obtaininformation of the position of the vehicle 500 based on the GPSpositioning data and the inertial navigation positioning data asdescribed above. The posture estimation device 1 can estimateinformation of the posture of the vehicle 500 based on angular velocitydata and the like included in the inertial navigation positioning data,for example. The information of the posture of the vehicle 500 can berepresented by quaternion or by a roll angle, a pitch angle, and a yawangle, for example. The control device 570 controls the posture of thevehicle 500 based on the posture information of the vehicle 500, whichhas been estimated by the processing of the posture estimation device 1.The control is performed by the vehicle control unit 572, for example.The control of the posture can be implemented by the control device 570controlling the steering mechanism 584, for example. Alternatively, incontrol (such as slip control) for stabilizing the posture of thevehicle 500, the control device 570 may control the driving mechanism580 or control the braking mechanism 582. According to the embodiment,with the posture estimation device 1, it is possible to estimateinformation of a posture with high accuracy. Accordingly, it is possibleto realize appropriate posture control of the vehicle 500.

In the embodiment, the control device 570 controls at least one ofaccelerating, braking, and steering of the vehicle 500 based oninformation of the position and the posture of the vehicle 500, whichhas been obtained by the posture estimation device 1. For example, thecontrol device 570 controls at least one of the driving mechanism 580,the braking mechanism 582, and the steering mechanism 584 based on theinformation of the position and the posture of the vehicle 500. Thus,for example, it is possible to realize automatic driving control of thevehicle 500 by the automatic driving control unit 574. In the automaticdriving control, a monitoring result of a surrounding object by themonitoring device 578, map information or running route informationstored in the storage unit 576, and the like are used in addition to theinformation of the position and the posture of the vehicle 500. Thecontrol device 570 switches the execution or non-execution of theautomatic driving of the vehicle 500 based on a monitoring result of theoutput signal of the inertial measurement unit 10. For example, theposture estimation device 1 monitors the output signal such as detectiondata from the inertial measurement unit 10. When a decrease of detectionaccuracy of the inertial measurement unit 10 or a sensing problem isdetected based on the monitoring result, for example, the control device570 performs switching from the execution of the automatic driving tothe non-execution of the automatic driving. For example, in theautomatic driving, at least one of accelerating, braking, and steeringof the vehicle 500 is automatically controlled. When the automaticdriving is not executed, such automatic control of accelerating,braking, and steering is not performed. In this manner, an assistancehaving higher reliability in running of the vehicle 500 that performsautomatic driving is possible. An automation level of the automaticdriving may be switched based on the monitoring result of the outputsignal of the inertial measurement unit 10.

FIG. 15 illustrates an example of another vehicle 600. FIG. 16 is ablock diagram illustrating an example of a configuration of the vehicle600. The posture estimation device 1 in the embodiment can beeffectively used in posture control and the like of a constructionmachine. FIGS. 15 and 16 illustrate a hydraulic shovel being an exampleof the construction machine as the vehicle 600.

As illustrated in FIG. 15, in the vehicle 600, a vehicle body isconstituted by a lower running body 612 and an upper revolving body 611,and a work mechanism 620 is provided in the front of the upper revolvingbody 611. The upper revolving body 611 is mounted to be capable ofrevolving on the lower running body 612. The work mechanism 620 isconstituted by a plurality of members capable of pivoting in anup-and-down direction. A driver seat (not illustrated) is provided inthe upper revolving body 611. An operation device (not illustrated) thatoperates the members constituting the work mechanism 620 is provided onthe driver seat. An inertial measurement unit 10 d functioning as aninclination sensor that detects an inclination angle of the upperrevolving body 611 is disposed in the upper revolving body 611.

The work mechanism 620 includes a boom 613, an arm 614, a bucket link616, a bucket 615, a boom cylinder 617, an arm cylinder 618, and abucket cylinder 619, as the plurality of members. The boom 613 isattached to the front portion of the upper revolving body 611 to becapable of elevating. The arm 614 is attached to the tip of the boom 613to be capable of elevating. The bucket link 616 is attached to the tipof the arm 614 to be rotatable. The bucket 615 is attached to the tip ofthe arm 614 and the bucket link 616 to be rotatable. The boom cylinder617 drives the boom 613. The arm cylinder 618 drives the arm 614. Thebucket cylinder 619 drives the bucket 615 through the bucket link 616.

The base end of the boom 613 is supported by the upper revolving body611 to be rotatable in the up-and-down direction. The boom 613 isrotationally driven relative to the upper revolving body 611 byexpansion and contraction of the boom cylinder 617. An inertialmeasurement unit 10 c functioning as an inertial sensor that detects themotion state of the boom 613 is disposed in the boom 613.

One end of the arm 614 is supported by the tip of the boom 613 to berotatable. The arm 614 is rotationally driven relative to the boom 613by expansion and contraction of the arm cylinder 618. An inertialmeasurement unit 10 b functioning as an inertial sensor that detects themotion state of the arm 614 is disposed in the arm 614.

The bucket link 616 and the bucket 615 are supported by the tip of thearm 614 to be rotatable. The bucket link 616 is rotationally drivenrelative to the arm 614 by expansion and contraction of the bucketcylinder 619. The bucket 615 is rotationally driven relative to the arm614 with the bucket link 616 driven. An inertial measurement unit 10 afunctioning as an inertial sensor that detects the motion state of thebucket link 616 is disposed in the bucket link 616.

Here, the inertial measurement unit 10 described in the above embodimentcan be used as the inertial measurement units 10 a, 10 b, 10 c, and 10d. The inertial measurement units 10 a, 10 b, 10 c, and 10 d can detectat least any of an angular velocity and an acceleration acting on themembers of the work mechanism 620 or the upper revolving body 611. Asillustrated in FIG. 16, the inertial measurement units 10 a, 10 b, and10 c are coupled in series and can transmit a detection signal to acalculation device 630. As described above, since the inertialmeasurement units 10 a, 10 b, and 10 c are coupled in series, it ispossible to reduce the number of wires for transmitting a detectionsignal in a movable region and to obtain a compact wiring structure.With the compact wiring structure, it is easy to select a method oflaying the wire, and it is possible to reduce an occurrence ofdeteriorating or damaging the wire, for example.

Further, as illustrated in FIG. 15, the calculation device 630 isprovided in the vehicle 600. The calculation device 630 calculates aninclination angle of the upper revolving body 611 or the positions orpostures of the boom 613, the arm 614, and the bucket 615 constitutingthe work mechanism 620. As illustrated in FIG. 16, the calculationdevice 630 includes the posture estimation device 1 in the aboveembodiment and a control device 632. The posture estimation device 1estimates posture information of the vehicle 600 based on an outputsignal of the inertial measurement units 10 a, 10 b, 10 c, and 10 d. Thecontrol device 632 controls the posture of the vehicle 600 based on theposture information of the vehicle 600, which has been estimated by theposture estimation device 1. Specifically, the calculation device 630receives various detection signals input from the inertial measurementunits 10 a, 10 b, 10 c, and 10 d and calculates the positions andpostures (posture angles) of the boom 613, the arm 614, and the bucket615 or an inclination state of the upper revolving body 611 based on thevarious detection signals. The calculated position-and-posture signalincluding a posture angle of the boom 613, the arm 614, or the bucket615 or an inclination signal including a posture angle of the upperrevolving body 611, for example, the position-and-posture signal of thebucket 615 is used in feedback information for a display of a monitoringdevice (not illustrated) on the driver seat or for controlling anoperation of the work mechanism 620 or the upper revolving body 611.

As the construction machine in which the posture estimation device 1 inthe above embodiment is used, for example, a rough terrain crane (cranecar), a bulldozer, an excavator/loader, a wheel loader, and an aerialwork vehicle (lift car) are provided in addition to the hydraulic shovel(jumbo, back hoe, and power shovel) exemplified above.

According to the embodiment, with the posture estimation device 1, it ispossible to obtain information of a posture with high accuracy. Thus, itis possible to realize appropriate posture control of the vehicle 600.According to the vehicle 600, since the compact inertial measurementunit 10 is mounted, it is possible to provide a construction machine inwhich a plurality of inertial measurement units can be compactlydisposed at installation sites of the inertial measurement units 10 byserial coupling (multi-coupling), or cable routing of coupling theinertial measurement units 10 installed at the sites to each other inseries by a cable can be compactly performed, even in a very narrowregion such as the bucket link 616.

In the embodiment, descriptions are made by using the four-wheel vehiclesuch as the agricultural machine and the construction machine as anexample of the vehicle in which the posture estimation device 1 is used.However, in addition, motorcycles, bicycles, trains, airplanes, bipedrobots, remote-controlled or autonomous aircraft (such asradio-controlled aircraft, radio-controlled helicopters and drones),rockets, satellites, ships, automated guided vehicles (AGVs) areprovided.

The present disclosure is not limited to the embodiment, and variousmodifications can be made in a range of the gist of the presentdisclosure.

The above-described embodiment and modification examples are examples,and the present disclosure is not limited thereto. For example, theembodiment and the modification examples can be appropriately combined.

The present disclosure includes a configuration which is substantiallythe same as the configuration described in the embodiment (for example,configuration having the same function, method, and result orconfiguration having the same purpose and effect). The presentdisclosure includes a configuration in which the not-essential componentin the configuration described in the embodiment has been replaced. Thepresent disclosure includes a configuration of exhibiting the sameeffects as those in the configuration described in the embodiment or aconfiguration capable of achieving the same purpose. The presentdisclosure includes a configuration in which a known technique is addedto the configuration described in the embodiment.

What is claimed is:
 1. A posture estimation method comprising:calculating a posture change amount of an object based on an output ofan angular velocity sensor; predicting posture information of the objectby using the posture change amount; adjusting error information in amanner of determining whether or not the output of the angular velocitysensor is within an effective range, and when it is determined that theoutput of the angular velocity sensor is not within the effective range,increasing a posture error component in error information and reducing acorrelation component between the posture error component and an errorcomponent other than the posture error component in the errorinformation; and correcting the predicted posture information of theobject based on the error information.
 2. The posture estimation methodaccording to claim 1, wherein the adjusting of the error informationincludes determining whether or not a current time is in a first periodafter it is determined that the output of the angular velocity sensor isnot within the effective range, increasing the posture error componentin the first period, and reducing the correlation component between theposture error component and the error component other than the postureerror component, in the first period.
 3. The posture estimation methodaccording to claim 1, wherein the error component other than the postureerror component includes a bias error component of an angular velocity.4. The posture estimation method according to claim 2, wherein the errorcomponent other than the posture error component includes a bias errorcomponent of an angular velocity.
 5. The posture estimation methodaccording to claim 1, wherein the correlation component between theposture error component and the error component other than the postureerror component is zero.
 6. The posture estimation method according toclaim 1, further comprising: calculating a velocity change amount of theobject based on an output of an acceleration sensor and the output ofthe angular velocity sensor, wherein in the adjusting of the errorinformation, whether or not the output of the acceleration sensor iswithin an effective range is determined, and, when it is determined thatthe output of the angular velocity sensor or the output of theacceleration sensor is not within the corresponding effective range, amotion velocity error component in the error information is increased,and a correlation component between the motion velocity error componentand an error component other than the motion velocity error component inthe error information is reduced, and in the predicting of the postureinformation, velocity information of the object is predicted by usingthe velocity change amount.
 7. The posture estimation method accordingto claim 6, wherein the adjusting of the error information includesdetermining whether or not a current time is in a second period after itis determined that the output of the acceleration sensor is not withinthe effective range, increasing the motion velocity error component inthe second period, and reducing the correlation component between themotion velocity error component and the error component other than themotion velocity error component, in the second period.
 8. The postureestimation method according to claim 6, wherein the error componentother than the motion velocity error component includes a bias errorcomponent of an acceleration.
 9. The posture estimation method accordingto claim 7, wherein the error component other than the motion velocityerror component includes a bias error component of an acceleration. 10.The posture estimation method according to claim 6, wherein thecorrelation component between the motion velocity error component andthe error component other than the motion velocity error component iszero.
 11. A posture estimation device comprising: aposture-change-amount calculation unit that calculates a posture changeamount of an object based on an output of an angular velocity sensor; aposture information prediction unit that predicts posture information ofthe object by using the posture change amount; an error informationadjustment unit that determines whether or not the output of the angularvelocity sensor is within an effective range, and when it is determinedthat the output of the angular velocity sensor is not within theeffective range, increases a posture error component in errorinformation, and reduces a correlation component between the postureerror component and an error component other than the posture errorcomponent in the error information; and a posture information correctionunit that corrects the predicted posture information of the object basedon the error information.
 12. A vehicle comprising: the postureestimation device according to claim 11; and a control device thatcontrols a posture of the vehicle based on posture information of thevehicle, which is estimated by the posture estimation device.